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

Example 16 with Quat4f

use of javax.vecmath.Quat4f in project Terasology by MovingBlocks.

the class BulletSweepCallback method checkForStep.

@Override
public boolean checkForStep(org.terasology.math.geom.Vector3f direction, float stepHeight, float slopeFactor, float checkForwardDistance) {
    boolean moveUpStep;
    boolean hitStep = false;
    float stepSlope = 1f;
    Vector3f lookAheadOffset = new Vector3f(direction.x, direction.y, direction.z);
    lookAheadOffset.y = 0;
    lookAheadOffset.normalize();
    lookAheadOffset.scale(checkForwardDistance);
    Vector3f fromWorld = new Vector3f(hitPointWorld);
    fromWorld.y += stepHeight + 0.05f;
    fromWorld.add(lookAheadOffset);
    Vector3f toWorld = new Vector3f(hitPointWorld);
    toWorld.y -= 0.05f;
    toWorld.add(lookAheadOffset);
    CollisionWorld.ClosestRayResultCallback rayResult = new CollisionWorld.ClosestRayResultCallback(fromWorld, toWorld);
    Transform transformFrom = new Transform(new Matrix4f(new Quat4f(0, 0, 0, 1), fromWorld, 1.0f));
    Transform transformTo = new Transform(new Matrix4f(new Quat4f(0, 0, 0, 1), toWorld, 1.0f));
    Transform targetTransform = this.hitCollisionObject.getWorldTransform(new Transform());
    CollisionWorld.rayTestSingle(transformFrom, transformTo, this.hitCollisionObject, this.hitCollisionObject.getCollisionShape(), targetTransform, rayResult);
    if (rayResult.hasHit()) {
        hitStep = true;
        stepSlope = rayResult.hitNormalWorld.dot(new Vector3f(0, 1, 0));
    }
    fromWorld.add(lookAheadOffset);
    toWorld.add(lookAheadOffset);
    rayResult = new CollisionWorld.ClosestRayResultCallback(fromWorld, toWorld);
    transformFrom = new Transform(new Matrix4f(new Quat4f(0, 0, 0, 1), fromWorld, 1.0f));
    transformTo = new Transform(new Matrix4f(new Quat4f(0, 0, 0, 1), toWorld, 1.0f));
    targetTransform = this.hitCollisionObject.getWorldTransform(new Transform());
    CollisionWorld.rayTestSingle(transformFrom, transformTo, this.hitCollisionObject, this.hitCollisionObject.getCollisionShape(), targetTransform, rayResult);
    if (rayResult.hasHit()) {
        hitStep = true;
        stepSlope = Math.min(stepSlope, rayResult.hitNormalWorld.dot(new Vector3f(0, 1, 0)));
    }
    moveUpStep = hitStep && stepSlope >= slopeFactor;
    return moveUpStep;
}

Example 17 with Quat4f

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

the class ConeTwistConstraint method buildJacobian.

@Override
public void buildJacobian() {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Vector3f tmp1 = stack.allocVector3f();
    Vector3f tmp2 = stack.allocVector3f();
    Transform tmpTrans = stack.allocTransform();
    appliedImpulse = 0f;
    // set bias, sign, clear accumulator
    swingCorrection = 0f;
    twistLimitSign = 0f;
    solveTwistLimit = false;
    solveSwingLimit = false;
    accTwistLimitImpulse = 0f;
    accSwingLimitImpulse = 0f;
    if (!angularOnly) {
        Vector3f pivotAInW = stack.alloc(rbAFrame.origin);
        rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);
        Vector3f pivotBInW = stack.alloc(rbBFrame.origin);
        rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);
        Vector3f relPos = stack.allocVector3f();
        relPos.sub(pivotBInW, pivotAInW);
        // TODO: stack
        Vector3f[] normal = /*[3]*/
        new Vector3f[] { stack.allocVector3f(), stack.allocVector3f(), stack.allocVector3f() };
        if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
            normal[0].normalize(relPos);
        } else {
            normal[0].set(1f, 0f, 0f);
        }
        TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);
        for (int i = 0; i < 3; i++) {
            Matrix3f mat1 = rbA.getCenterOfMassTransform(stack.allocTransform()).basis;
            mat1.transpose();
            Matrix3f mat2 = rbB.getCenterOfMassTransform(stack.allocTransform()).basis;
            mat2.transpose();
            tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmp));
            tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmp));
            jac[i].init(mat1, mat2, tmp1, tmp2, normal[i], rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvMass());
        }
    }
    Vector3f b1Axis1 = stack.allocVector3f(), b1Axis2 = stack.allocVector3f(), b1Axis3 = stack.allocVector3f();
    Vector3f b2Axis1 = stack.allocVector3f(), b2Axis2 = stack.allocVector3f();
    rbAFrame.basis.getColumn(0, b1Axis1);
    getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis1);
    rbBFrame.basis.getColumn(0, b2Axis1);
    getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis1);
    float swing1 = 0f, swing2 = 0f;
    float swx = 0f, swy = 0f;
    float thresh = 10f;
    float fact;
    // Get Frame into world space
    if (swingSpan1 >= 0.05f) {
        rbAFrame.basis.getColumn(1, b1Axis2);
        getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis2);
        //			swing1 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis2), b2Axis1.dot(b1Axis1));
        swx = b2Axis1.dot(b1Axis1);
        swy = b2Axis1.dot(b1Axis2);
        swing1 = ScalarUtil.atan2Fast(swy, swx);
        fact = (swy * swy + swx * swx) * thresh * thresh;
        fact = fact / (fact + 1f);
        swing1 *= fact;
    }
    if (swingSpan2 >= 0.05f) {
        rbAFrame.basis.getColumn(2, b1Axis3);
        getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis3);
        //			swing2 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis3), b2Axis1.dot(b1Axis1));
        swx = b2Axis1.dot(b1Axis1);
        swy = b2Axis1.dot(b1Axis3);
        swing2 = ScalarUtil.atan2Fast(swy, swx);
        fact = (swy * swy + swx * swx) * thresh * thresh;
        fact = fact / (fact + 1f);
        swing2 *= fact;
    }
    float RMaxAngle1Sq = 1.0f / (swingSpan1 * swingSpan1);
    float RMaxAngle2Sq = 1.0f / (swingSpan2 * swingSpan2);
    float EllipseAngle = Math.abs(swing1 * swing1) * RMaxAngle1Sq + Math.abs(swing2 * swing2) * RMaxAngle2Sq;
    if (EllipseAngle > 1.0f) {
        swingCorrection = EllipseAngle - 1.0f;
        solveSwingLimit = true;
        // Calculate necessary axis & factors
        tmp1.scale(b2Axis1.dot(b1Axis2), b1Axis2);
        tmp2.scale(b2Axis1.dot(b1Axis3), b1Axis3);
        tmp.add(tmp1, tmp2);
        swingAxis.cross(b2Axis1, tmp);
        swingAxis.normalize();
        float swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
        swingAxis.scale(swingAxisSign);
        kSwing = 1f / (getRigidBodyA().computeAngularImpulseDenominator(swingAxis) + getRigidBodyB().computeAngularImpulseDenominator(swingAxis));
    }
    // Twist limits
    if (twistSpan >= 0f) {
        //Vector3f b2Axis2 = stack.allocVector3f();
        rbBFrame.basis.getColumn(1, b2Axis2);
        getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis2);
        Quat4f rotationArc = QuaternionUtil.shortestArcQuat(b2Axis1, b1Axis1, stack.allocQuat4f());
        Vector3f TwistRef = QuaternionUtil.quatRotate(rotationArc, b2Axis2, stack.allocVector3f());
        float twist = ScalarUtil.atan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
        float lockedFreeFactor = (twistSpan > 0.05f) ? limitSoftness : 0f;
        if (twist <= -twistSpan * lockedFreeFactor) {
            twistCorrection = -(twist + twistSpan);
            solveTwistLimit = true;
            twistAxis.add(b2Axis1, b1Axis1);
            twistAxis.scale(0.5f);
            twistAxis.normalize();
            twistAxis.scale(-1.0f);
            kTwist = 1f / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
        } else if (twist > twistSpan * lockedFreeFactor) {
            twistCorrection = (twist - twistSpan);
            solveTwistLimit = true;
            twistAxis.add(b2Axis1, b1Axis1);
            twistAxis.scale(0.5f);
            twistAxis.normalize();
            kTwist = 1f / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
        }
    }
    stack.leave();
}

Example 18 with Quat4f

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

the class Stack method allocQuat4f.

public Quat4f allocQuat4f() {
    types[sp++] = TYPE_QUAT4F;
    int pos = stackPositions[TYPE_QUAT4F]++;
    if (quat4fStack.size() <= pos) {
        quat4fStack.add(new Quat4f());
    }
    return quat4fStack.get(pos);
}

Example 19 with Quat4f

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

the class Stack method alloc.

public Quat4f alloc(Quat4f rotation) {
    Quat4f q = allocQuat4f();
    q.set(rotation);
    return q;
}

Example 20 with Quat4f

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

the class BuildCraftBakedModel method getItemTransforms.

@SuppressWarnings("deprecation")
protected static /**
 * Get the default transformations for inside inventories and third person
 */
ImmutableMap<TransformType, TRSRTransformation> getItemTransforms() {
    ImmutableMap.Builder<TransformType, TRSRTransformation> builder = ImmutableMap.builder();
    float scale = 0.375f;
    Vector3f translation = new Vector3f(0, 1.5F * scale, -2.75F * scale);
    TRSRTransformation trsr = new TRSRTransformation(translation, new Quat4f(10, -45, 170, 1), new Vector3f(0.375F, 0.375F, 0.375F), null);
    builder.put(TransformType.THIRD_PERSON, trsr);
    translation = new Vector3f(1, 1, 0);
    trsr = new TRSRTransformation(translation, new Quat4f(0, 0, 0, 1), new Vector3f(1, 1, 1), new Quat4f(0, -90, 90, 1));
    builder.put(TransformType.GUI, trsr);
    return builder.build();
}