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

Example 21 with Matrix3f

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

the class TriangleMeshShape method getAabb.

@Override
public void getAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Vector3f localHalfExtents = stack.allocVector3f();
    localHalfExtents.sub(localAabbMax, localAabbMin);
    localHalfExtents.scale(0.5f);
    Vector3f localCenter = stack.allocVector3f();
    localCenter.add(localAabbMax, localAabbMin);
    localCenter.scale(0.5f);
    Matrix3f abs_b = stack.alloc(trans.basis);
    MatrixUtil.absolute(abs_b);
    Vector3f center = stack.alloc(localCenter);
    trans.transform(center);
    Vector3f extent = stack.allocVector3f();
    abs_b.getRow(0, tmp);
    extent.x = tmp.dot(localHalfExtents);
    abs_b.getRow(1, tmp);
    extent.y = tmp.dot(localHalfExtents);
    abs_b.getRow(2, tmp);
    extent.z = tmp.dot(localHalfExtents);
    Vector3f margin = stack.allocVector3f();
    margin.set(getMargin(), getMargin(), getMargin());
    extent.add(margin);
    aabbMin.sub(center, extent);
    aabbMax.add(center, extent);
    stack.leave();
}

Example 22 with Matrix3f

use of javax.vecmath.Matrix3f 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 23 with Matrix3f

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

the class ContactConstraint method resolveSingleBilateral.

/**
	 * Bilateral constraint between two dynamic objects.
	 */
public static void resolveSingleBilateral(RigidBody body1, Vector3f pos1, RigidBody body2, Vector3f pos2, float distance, Vector3f normal, float[] impulse, float timeStep) {
    float normalLenSqr = normal.lengthSquared();
    assert (Math.abs(normalLenSqr) < 1.1f);
    if (normalLenSqr > 1.1f) {
        impulse[0] = 0f;
        return;
    }
    ObjectPool<JacobianEntry> jacobiansPool = ObjectPool.get(JacobianEntry.class, new Supplier<JacobianEntry>() {

        @Override
        public JacobianEntry get() {
            return new JacobianEntry();
        }
    });
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Vector3f rel_pos1 = stack.allocVector3f();
    rel_pos1.sub(pos1, body1.getCenterOfMassPosition(tmp));
    Vector3f rel_pos2 = stack.allocVector3f();
    rel_pos2.sub(pos2, body2.getCenterOfMassPosition(tmp));
    //this jacobian entry could be re-used for all iterations
    Vector3f vel1 = stack.allocVector3f();
    body1.getVelocityInLocalPoint(rel_pos1, vel1);
    Vector3f vel2 = stack.allocVector3f();
    body2.getVelocityInLocalPoint(rel_pos2, vel2);
    Vector3f vel = stack.allocVector3f();
    vel.sub(vel1, vel2);
    Matrix3f mat1 = body1.getCenterOfMassTransform(stack.allocTransform()).basis;
    mat1.transpose();
    Matrix3f mat2 = body2.getCenterOfMassTransform(stack.allocTransform()).basis;
    mat2.transpose();
    JacobianEntry jac = jacobiansPool.get();
    jac.init(mat1, mat2, rel_pos1, rel_pos2, normal, body1.getInvInertiaDiagLocal(stack.allocVector3f()), body1.getInvMass(), body2.getInvInertiaDiagLocal(stack.allocVector3f()), body2.getInvMass());
    float jacDiagAB = jac.getDiagonal();
    float jacDiagABInv = 1f / jacDiagAB;
    Vector3f tmp1 = body1.getAngularVelocity(stack.allocVector3f());
    mat1.transform(tmp1);
    Vector3f tmp2 = body2.getAngularVelocity(stack.allocVector3f());
    mat2.transform(tmp2);
    float rel_vel = jac.getRelativeVelocity(body1.getLinearVelocity(stack.allocVector3f()), tmp1, body2.getLinearVelocity(stack.allocVector3f()), tmp2);
    jacobiansPool.release(jac);
    float a;
    a = jacDiagABInv;
    rel_vel = normal.dot(vel);
    // todo: move this into proper structure
    float contactDamping = 0.2f;
    //#ifdef ONLY_USE_LINEAR_MASS
    //	btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
    //	impulse = - contactDamping * rel_vel * massTerm;
    //#else	
    float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
    impulse[0] = velocityImpulse;
    //#endif
    stack.leave();
}

Example 24 with Matrix3f

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

the class Generic6DofConstraint method buildLinearJacobian.

protected void buildLinearJacobian(/*JacobianEntry jacLinear*/
int jacLinear_index, Vector3f normalWorld, Vector3f pivotAInW, Vector3f pivotBInW) {
    Stack stack = Stack.enter();
    Matrix3f mat1 = rbA.getCenterOfMassTransform(stack.allocTransform()).basis;
    mat1.transpose();
    Matrix3f mat2 = rbB.getCenterOfMassTransform(stack.allocTransform()).basis;
    mat2.transpose();
    Vector3f tmpVec = stack.allocVector3f();
    Vector3f tmp1 = stack.allocVector3f();
    tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
    Vector3f tmp2 = stack.allocVector3f();
    tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
    jacLinear[jacLinear_index].init(mat1, mat2, tmp1, tmp2, normalWorld, rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvMass());
    stack.leave();
}

Example 25 with Matrix3f

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

the class HingeConstraint method buildJacobian.

@Override
public void buildJacobian() {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Vector3f tmp1 = stack.allocVector3f();
    Vector3f tmp2 = stack.allocVector3f();
    Vector3f tmpVec = stack.allocVector3f();
    Matrix3f mat1 = stack.allocMatrix3f();
    Matrix3f mat2 = stack.allocMatrix3f();
    Transform centerOfMassA = rbA.getCenterOfMassTransform(stack.allocTransform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(stack.allocTransform());
    appliedImpulse = 0f;
    if (!angularOnly) {
        Vector3f pivotAInW = stack.alloc(rbAFrame.origin);
        centerOfMassA.transform(pivotAInW);
        Vector3f pivotBInW = stack.alloc(rbBFrame.origin);
        centerOfMassB.transform(pivotBInW);
        Vector3f relPos = stack.allocVector3f();
        relPos.sub(pivotBInW, pivotAInW);
        Vector3f[] normal = /*[3]*/
        new Vector3f[] { stack.allocVector3f(), stack.allocVector3f(), stack.allocVector3f() };
        if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
            normal[0].set(relPos);
            normal[0].normalize();
        } else {
            normal[0].set(1f, 0f, 0f);
        }
        TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);
        for (int i = 0; i < 3; i++) {
            mat1.transpose(centerOfMassA.basis);
            mat2.transpose(centerOfMassB.basis);
            tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
            tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
            jac[i].init(mat1, mat2, tmp1, tmp2, normal[i], rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvMass());
        }
    }
    // calculate two perpendicular jointAxis, orthogonal to hingeAxis
    // these two jointAxis require equal angular velocities for both bodies
    // this is unused for now, it's a todo
    Vector3f jointAxis0local = stack.allocVector3f();
    Vector3f jointAxis1local = stack.allocVector3f();
    rbAFrame.basis.getColumn(2, tmp);
    TransformUtil.planeSpace1(tmp, jointAxis0local, jointAxis1local);
    // TODO: check this
    //getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
    Vector3f jointAxis0 = stack.alloc(jointAxis0local);
    centerOfMassA.basis.transform(jointAxis0);
    Vector3f jointAxis1 = stack.alloc(jointAxis1local);
    centerOfMassA.basis.transform(jointAxis1);
    Vector3f hingeAxisWorld = stack.allocVector3f();
    rbAFrame.basis.getColumn(2, hingeAxisWorld);
    centerOfMassA.basis.transform(hingeAxisWorld);
    mat1.transpose(centerOfMassA.basis);
    mat2.transpose(centerOfMassB.basis);
    jacAng[0].init(jointAxis0, mat1, mat2, rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvInertiaDiagLocal(stack.allocVector3f()));
    // JAVA NOTE: reused mat1 and mat2, as recomputation is not needed
    jacAng[1].init(jointAxis1, mat1, mat2, rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvInertiaDiagLocal(stack.allocVector3f()));
    // JAVA NOTE: reused mat1 and mat2, as recomputation is not needed
    jacAng[2].init(hingeAxisWorld, mat1, mat2, rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvInertiaDiagLocal(stack.allocVector3f()));
    // Compute limit information
    float hingeAngle = getHingeAngle();
    //set bias, sign, clear accumulator
    correction = 0f;
    limitSign = 0f;
    solveLimit = false;
    accLimitImpulse = 0f;
    if (lowerLimit < upperLimit) {
        if (hingeAngle <= lowerLimit * limitSoftness) {
            correction = (lowerLimit - hingeAngle);
            limitSign = 1.0f;
            solveLimit = true;
        } else if (hingeAngle >= upperLimit * limitSoftness) {
            correction = upperLimit - hingeAngle;
            limitSign = -1.0f;
            solveLimit = true;
        }
    }
    // Compute K = J*W*J' for hinge axis
    Vector3f axisA = stack.allocVector3f();
    rbAFrame.basis.getColumn(2, axisA);
    centerOfMassA.basis.transform(axisA);
    kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + getRigidBodyB().computeAngularImpulseDenominator(axisA));
    stack.leave();
}