Examples with Vector3f javax.vecmath.Vector3f used on opensource projects

Example 86 with Vector3f

use of javax.vecmath.Vector3f in project bdx by GoranM.

the class SliderConstraint method getAncorInA.

// access for PE Solver
public Vector3f getAncorInA(Vector3f out) {
    Stack stack = Stack.enter();
    Transform tmpTrans = stack.allocTransform();
    Vector3f ancorInA = out;
    ancorInA.scaleAdd((lowerLinLimit + upperLinLimit) * 0.5f, sliderAxis, realPivotAInW);
    rbA.getCenterOfMassTransform(tmpTrans);
    tmpTrans.inverse();
    tmpTrans.transform(ancorInA);
    stack.leave();
    return ancorInA;
}

Example 87 with Vector3f

use of javax.vecmath.Vector3f in project bdx by GoranM.

the class SliderConstraint method calculateTransforms.

// shared code used by ODE solver
public void calculateTransforms() {
    Stack stack = Stack.enter();
    Transform tmpTrans = stack.allocTransform();
    if (useLinearReferenceFrameA) {
        calculatedTransformA.mul(rbA.getCenterOfMassTransform(tmpTrans), frameInA);
        calculatedTransformB.mul(rbB.getCenterOfMassTransform(tmpTrans), frameInB);
    } else {
        calculatedTransformA.mul(rbB.getCenterOfMassTransform(tmpTrans), frameInB);
        calculatedTransformB.mul(rbA.getCenterOfMassTransform(tmpTrans), frameInA);
    }
    realPivotAInW.set(calculatedTransformA.origin);
    realPivotBInW.set(calculatedTransformB.origin);
    // along X
    calculatedTransformA.basis.getColumn(0, sliderAxis);
    delta.sub(realPivotBInW, realPivotAInW);
    projPivotInW.scaleAdd(sliderAxis.dot(delta), sliderAxis, realPivotAInW);
    Vector3f normalWorld = stack.allocVector3f();
    // linear part
    for (int i = 0; i < 3; i++) {
        calculatedTransformA.basis.getColumn(i, normalWorld);
        VectorUtil.setCoord(depth, i, delta.dot(normalWorld));
    }
    stack.leave();
}

Example 88 with Vector3f

use of javax.vecmath.Vector3f in project bdx by GoranM.

the class SolverBody method getVelocityInLocalPoint.

public void getVelocityInLocalPoint(Vector3f rel_pos, Vector3f velocity) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    tmp.cross(angularVelocity, rel_pos);
    velocity.add(linearVelocity, tmp);
    stack.leave();
}

Example 89 with Vector3f

use of javax.vecmath.Vector3f in project bdx by GoranM.

the class RaycastVehicle method updateWheelTransform.

public void updateWheelTransform(int wheelIndex, boolean interpolatedTransform) {
    WheelInfo wheel = wheelInfo.getQuick(wheelIndex);
    updateWheelTransformsWS(wheel, interpolatedTransform);
    Stack stack = Stack.enter();
    Vector3f up = stack.allocVector3f();
    up.negate(wheel.raycastInfo.wheelDirectionWS);
    Vector3f right = wheel.raycastInfo.wheelAxleWS;
    Vector3f fwd = stack.allocVector3f();
    fwd.cross(up, right);
    fwd.normalize();
    // up = right.cross(fwd);
    // up.normalize();
    // rotate around steering over de wheelAxleWS
    float steering = wheel.steering;
    Quat4f steeringOrn = stack.allocQuat4f();
    //wheel.m_steering);
    QuaternionUtil.setRotation(steeringOrn, up, steering);
    Matrix3f steeringMat = stack.allocMatrix3f();
    MatrixUtil.setRotation(steeringMat, steeringOrn);
    Quat4f rotatingOrn = stack.allocQuat4f();
    QuaternionUtil.setRotation(rotatingOrn, right, -wheel.rotation);
    Matrix3f rotatingMat = stack.allocMatrix3f();
    MatrixUtil.setRotation(rotatingMat, rotatingOrn);
    Matrix3f basis2 = stack.allocMatrix3f();
    basis2.setRow(0, right.x, fwd.x, up.x);
    basis2.setRow(1, right.y, fwd.y, up.y);
    basis2.setRow(2, right.z, fwd.z, up.z);
    Matrix3f wheelBasis = wheel.worldTransform.basis;
    wheelBasis.mul(steeringMat, rotatingMat);
    wheelBasis.mul(basis2);
    wheel.worldTransform.origin.scaleAdd(wheel.raycastInfo.suspensionLength, wheel.raycastInfo.wheelDirectionWS, wheel.raycastInfo.hardPointWS);
    stack.leave();
}

Example 90 with Vector3f

use of javax.vecmath.Vector3f in project bdx by GoranM.

the class RaycastVehicle method updateVehicle.

public void updateVehicle(float step) {
    for (int i = 0; i < getNumWheels(); i++) {
        updateWheelTransform(i, false);
    }
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    currentVehicleSpeedKmHour = 3.6f * getRigidBody().getLinearVelocity(tmp).length();
    Transform chassisTrans = getChassisWorldTransform(stack.allocTransform());
    Vector3f forwardW = stack.allocVector3f();
    forwardW.set(chassisTrans.basis.getElement(0, indexForwardAxis), chassisTrans.basis.getElement(1, indexForwardAxis), chassisTrans.basis.getElement(2, indexForwardAxis));
    if (forwardW.dot(getRigidBody().getLinearVelocity(tmp)) < 0f) {
        currentVehicleSpeedKmHour *= -1f;
    }
    //
    // simulate suspension
    //
    int i = 0;
    for (i = 0; i < wheelInfo.size(); i++) {
        float depth;
        depth = rayCast(wheelInfo.getQuick(i));
    }
    updateSuspension(step);
    for (i = 0; i < wheelInfo.size(); i++) {
        // apply suspension force
        WheelInfo wheel = wheelInfo.getQuick(i);
        float suspensionForce = wheel.wheelsSuspensionForce;
        float gMaxSuspensionForce = 6000f;
        if (suspensionForce > gMaxSuspensionForce) {
            suspensionForce = gMaxSuspensionForce;
        }
        Vector3f impulse = stack.allocVector3f();
        impulse.scale(suspensionForce * step, wheel.raycastInfo.contactNormalWS);
        Vector3f relpos = stack.allocVector3f();
        relpos.sub(wheel.raycastInfo.contactPointWS, getRigidBody().getCenterOfMassPosition(tmp));
        getRigidBody().applyImpulse(impulse, relpos);
    }
    updateFriction(step);
    for (i = 0; i < wheelInfo.size(); i++) {
        WheelInfo wheel = wheelInfo.getQuick(i);
        Vector3f relpos = stack.allocVector3f();
        relpos.sub(wheel.raycastInfo.hardPointWS, getRigidBody().getCenterOfMassPosition(tmp));
        Vector3f vel = getRigidBody().getVelocityInLocalPoint(relpos, stack.allocVector3f());
        if (wheel.raycastInfo.isInContact) {
            Transform chassisWorldTransform = getChassisWorldTransform(stack.allocTransform());
            Vector3f fwd = stack.allocVector3f();
            fwd.set(chassisWorldTransform.basis.getElement(0, indexForwardAxis), chassisWorldTransform.basis.getElement(1, indexForwardAxis), chassisWorldTransform.basis.getElement(2, indexForwardAxis));
            float proj = fwd.dot(wheel.raycastInfo.contactNormalWS);
            tmp.scale(proj, wheel.raycastInfo.contactNormalWS);
            fwd.sub(tmp);
            float proj2 = fwd.dot(vel);
            wheel.deltaRotation = (proj2 * step) / (wheel.wheelsRadius);
            wheel.rotation += wheel.deltaRotation;
        } else {
            wheel.rotation += wheel.deltaRotation;
        }
        // damping of rotation when not in contact
        wheel.deltaRotation *= 0.99f;
    }
    stack.leave();
}