Examples with Quat4f spacegraph.util.math.Quat4f used on opensource projects

Example 1 with Quat4f

use of spacegraph.util.math.Quat4f in project narchy by automenta.

the class QuaternionUtil method quatRotate.

public static v3 quatRotate(Quat4f rotation, v3 v, v3 out) {
    Quat4f q = new Quat4f(rotation);
    mul(q, v);
    Quat4f tmp = new Quat4f();
    inverse(tmp, rotation);
    q.mul(tmp);
    out.set(q.x, q.y, q.z);
    return out;
}

Example 2 with Quat4f

use of spacegraph.util.math.Quat4f in project narchy by automenta.

the class ConeTwistConstraint method buildJacobian.

@Override
public void buildJacobian() {
    v3 tmp = new v3();
    v3 tmp1 = new v3();
    v3 tmp2 = new v3();
    Transform tmpTrans = new Transform();
    appliedImpulse = 0f;
    // set bias, sign, clear accumulator
    swingCorrection = 0f;
    twistLimitSign = 0f;
    solveTwistLimit = false;
    solveSwingLimit = false;
    accTwistLimitImpulse = 0f;
    accSwingLimitImpulse = 0f;
    if (!angularOnly) {
        v3 pivotAInW = new v3(rbAFrame);
        rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);
        v3 pivotBInW = new v3(rbBFrame);
        rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);
        v3 relPos = new v3();
        relPos.sub(pivotBInW, pivotAInW);
        // TODO: stack
        v3[] normal = /*[3]*/
        { new v3(), new v3(), new v3() };
        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(new Transform()).basis;
            mat1.transpose();
            Matrix3f mat2 = rbB.getCenterOfMassTransform(new Transform()).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(new v3()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(new v3()), rbB.getInvMass());
        }
    }
    v3 b1Axis1 = new v3(), b1Axis2 = new v3(), b1Axis3 = new v3();
    v3 b2Axis1 = new v3(), b2Axis2 = new v3();
    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 = new Vector3f();
        rbBFrame.basis.getColumn(1, b2Axis2);
        getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis2);
        Quat4f rotationArc = QuaternionUtil.shortestArcQuat(b2Axis1, b1Axis1, new Quat4f());
        v3 TwistRef = QuaternionUtil.quatRotate(rotationArc, b2Axis2, new v3());
        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));
        }
    }
}

Example 3 with Quat4f

use of spacegraph.util.math.Quat4f in project narchy by automenta.

the class GhostObject method convexSweepTest.

public void convexSweepTest(ConvexShape castShape, Transform convexFromWorld, Transform convexToWorld, Collisions.ConvexResultCallback resultCallback, float allowedCcdPenetration) {
    Transform convexFromTrans = new Transform();
    Transform convexToTrans = new Transform();
    convexFromTrans.set(convexFromWorld);
    convexToTrans.set(convexToWorld);
    v3 castShapeAabbMin = new v3();
    v3 castShapeAabbMax = new v3();
    // compute AABB that encompasses angular movement
    v3 linVel = new v3();
    v3 angVel = new v3();
    TransformUtil.calculateVelocity(convexFromTrans, convexToTrans, 1f, linVel, angVel);
    Transform R = new Transform();
    R.setIdentity();
    R.setRotation(convexFromTrans.getRotation(new Quat4f()));
    castShape.calculateTemporalAabb(R, linVel, angVel, 1f, castShapeAabbMin, castShapeAabbMax);
    Transform tmpTrans = new Transform();
    // do a ray-shape query using convexCaster (CCD)
    for (int i = 0; i < overlappingObjects.size(); i++) {
        // return array[index];
        Collidable collidable = overlappingObjects.get(i);
        // only perform raycast if filterMask matches
        if (resultCallback.needsCollision(collidable.broadphase)) {
            // RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
            v3 collisionObjectAabbMin = new v3();
            v3 collisionObjectAabbMax = new v3();
            collidable.shape().getAabb(collidable.getWorldTransform(tmpTrans), collisionObjectAabbMin, collisionObjectAabbMax);
            AabbUtil2.aabbExpand(collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
            // could use resultCallback.closestHitFraction, but needs testing
            float[] hitLambda = { 1f };
            v3 hitNormal = new v3();
            if (AabbUtil2.rayAabb(convexFromWorld, convexToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) {
                Collisions.objectQuerySingle(castShape, convexFromTrans, convexToTrans, collidable, collidable.shape(), collidable.getWorldTransform(tmpTrans), resultCallback, allowedCcdPenetration);
            }
        }
    }
}

Example 4 with Quat4f

use of spacegraph.util.math.Quat4f in project narchy by automenta.

the class TransformUtil method integrateTransform.

public static void integrateTransform(Transform curTrans, v3 linvel, v3 angvel, float timeStep, Transform predictedTransform) {
    predictedTransform.scaleAdd(timeStep, linvel, curTrans);
    // //#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
    v3 axis = new v3();
    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.5 * fAngle * timeStep) / fAngle, angvel);
    }
    Quat4f dorn = new Quat4f(axis.x, axis.y, axis.z, (float) Math.cos(0.5 * fAngle * timeStep));
    Quat4f orn0 = curTrans.getRotation(new Quat4f());
    Quat4f predictedOrn = new Quat4f();
    predictedOrn.mul(dorn, orn0);
    predictedOrn.normalize();
    // #endif
    predictedTransform.setRotation(predictedOrn);
}

Example 5 with Quat4f

use of spacegraph.util.math.Quat4f in project narchy by automenta.

the class TransformUtil method calculateDiffAxisAngle.

public static void calculateDiffAxisAngle(Transform transform0, Transform transform1, v3 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
    Matrix3f tmp = new Matrix3f();
    tmp.set(transform0.basis);
    MatrixUtil.invert(tmp);
    Matrix3f dmat = new Matrix3f();
    dmat.mul(transform1.basis, tmp);
    Quat4f dorn = new Quat4f();
    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 lenSq = axis.lengthSquared();
    if (lenSq < BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) {
        axis.set(1f, 0f, 0f);
    } else {
        axis.scale(1f / (float) Math.sqrt(lenSq));
    }
}