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

Example 1 with Quat4f

use of javax.vecmath.Quat4f in project MinecraftForge by MinecraftForge.

the class TRSRTransformation method quatFromYXZ.

public static Quat4f quatFromYXZ(float y, float x, float z) {
    Quat4f ret = new Quat4f(0, 0, 0, 1), t = new Quat4f();
    t.set(0, (float) Math.sin(y / 2), 0, (float) Math.cos(y / 2));
    ret.mul(t);
    t.set((float) Math.sin(x / 2), 0, 0, (float) Math.cos(x / 2));
    ret.mul(t);
    t.set(0, 0, (float) Math.sin(z / 2), (float) Math.cos(z / 2));
    ret.mul(t);
    return ret;
}

Example 2 with Quat4f

use of javax.vecmath.Quat4f 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 3 with Quat4f

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

the class QuaternionUtil method quatRotate.

public static Vector3f quatRotate(Quat4f rotation, Vector3f v, Vector3f out) {
    Stack stack = Stack.enter();
    Quat4f q = stack.alloc(rotation);
    QuaternionUtil.mul(q, v);
    Quat4f tmp = stack.allocQuat4f();
    inverse(tmp, rotation);
    q.mul(tmp);
    out.set(q.x, q.y, q.z);
    stack.leave();
    return out;
}

Example 4 with Quat4f

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

the class TransformUtil method integrateTransform.

@StaticAlloc
public static void integrateTransform(Transform curTrans, Vector3f linvel, Vector3f angvel, float timeStep, Transform predictedTransform) {
    predictedTransform.origin.scaleAdd(timeStep, linvel, curTrans.origin);
    //	//#define QUATERNION_DERIVATIVE
    //	#ifdef QUATERNION_DERIVATIVE
    //		btQuaternion predictedOrn = curTrans.getRotation();
    //		predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
    //		predictedOrn.normalize();
    //	#else
    // Exponential map
    // google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
    Stack stack = Stack.enter();
    Vector3f axis = stack.allocVector3f();
    float fAngle = angvel.length();
    // limit the angular motion
    if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD) {
        fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
    }
    if (fAngle < 0.001f) {
        // use Taylor's expansions of sync function
        axis.scale(0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * fAngle * fAngle, angvel);
    } else {
        // sync(fAngle) = sin(c*fAngle)/t
        axis.scale((float) Math.sin(0.5f * fAngle * timeStep) / fAngle, angvel);
    }
    Quat4f dorn = stack.allocQuat4f();
    dorn.set(axis.x, axis.y, axis.z, (float) Math.cos(fAngle * timeStep * 0.5f));
    Quat4f orn0 = curTrans.getRotation(stack.allocQuat4f());
    Quat4f predictedOrn = stack.allocQuat4f();
    predictedOrn.mul(dorn, orn0);
    predictedOrn.normalize();
    //  #endif
    predictedTransform.setRotation(predictedOrn);
    stack.leave();
}

Example 5 with Quat4f

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

the class TransformUtil method calculateDiffAxisAngle.

public static void calculateDiffAxisAngle(Transform transform0, Transform transform1, Vector3f axis, float[] angle) {
    // #ifdef USE_QUATERNION_DIFF
    //		btQuaternion orn0 = transform0.getRotation();
    //		btQuaternion orn1a = transform1.getRotation();
    //		btQuaternion orn1 = orn0.farthest(orn1a);
    //		btQuaternion dorn = orn1 * orn0.inverse();
    // #else
    Stack stack = Stack.enter();
    Matrix3f tmp = stack.allocMatrix3f();
    tmp.set(transform0.basis);
    MatrixUtil.invert(tmp);
    Matrix3f dmat = stack.allocMatrix3f();
    dmat.mul(transform1.basis, tmp);
    Quat4f dorn = stack.allocQuat4f();
    MatrixUtil.getRotation(dmat, dorn);
    // #endif
    // floating point inaccuracy can lead to w component > 1..., which breaks 
    dorn.normalize();
    angle[0] = QuaternionUtil.getAngle(dorn);
    axis.set(dorn.x, dorn.y, dorn.z);
    // TODO: probably not needed
    //axis[3] = btScalar(0.);
    // check for axis length
    float len = axis.lengthSquared();
    if (len < BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) {
        axis.set(1f, 0f, 0f);
    } else {
        axis.scale(1f / (float) Math.sqrt(len));
    }
    stack.leave();
}