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

Example 26 with Matrix3f

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

the class SequentialImpulseConstraintSolver method prepareConstraints.

protected void prepareConstraints(PersistentManifold manifoldPtr, ContactSolverInfo info, IDebugDraw debugDrawer) {
    Stack stack = Stack.enter();
    RigidBody body0 = (RigidBody) manifoldPtr.getBody0();
    RigidBody body1 = (RigidBody) manifoldPtr.getBody1();
    // only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
    {
        //#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
        //manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
        //#endif //FORCE_REFESH_CONTACT_MANIFOLDS		
        int numpoints = manifoldPtr.getNumContacts();
        BulletStats.gTotalContactPoints += numpoints;
        Vector3f tmpVec = stack.allocVector3f();
        Matrix3f tmpMat3 = stack.allocMatrix3f();
        Vector3f pos1 = stack.allocVector3f();
        Vector3f pos2 = stack.allocVector3f();
        Vector3f rel_pos1 = stack.allocVector3f();
        Vector3f rel_pos2 = stack.allocVector3f();
        Vector3f vel1 = stack.allocVector3f();
        Vector3f vel2 = stack.allocVector3f();
        Vector3f vel = stack.allocVector3f();
        Vector3f totalImpulse = stack.allocVector3f();
        Vector3f torqueAxis0 = stack.allocVector3f();
        Vector3f torqueAxis1 = stack.allocVector3f();
        Vector3f ftorqueAxis0 = stack.allocVector3f();
        Vector3f ftorqueAxis1 = stack.allocVector3f();
        for (int i = 0; i < numpoints; i++) {
            ManifoldPoint cp = manifoldPtr.getContactPoint(i);
            if (cp.getDistance() <= 0f) {
                cp.getPositionWorldOnA(pos1);
                cp.getPositionWorldOnB(pos2);
                rel_pos1.sub(pos1, body0.getCenterOfMassPosition(tmpVec));
                rel_pos2.sub(pos2, body1.getCenterOfMassPosition(tmpVec));
                // this jacobian entry is re-used for all iterations
                Matrix3f mat1 = body0.getCenterOfMassTransform(stack.allocTransform()).basis;
                mat1.transpose();
                Matrix3f mat2 = body1.getCenterOfMassTransform(stack.allocTransform()).basis;
                mat2.transpose();
                JacobianEntry jac = jacobiansPool.get();
                jac.init(mat1, mat2, rel_pos1, rel_pos2, cp.normalWorldOnB, body0.getInvInertiaDiagLocal(stack.allocVector3f()), body0.getInvMass(), body1.getInvInertiaDiagLocal(stack.allocVector3f()), body1.getInvMass());
                float jacDiagAB = jac.getDiagonal();
                jacobiansPool.release(jac);
                ConstraintPersistentData cpd = (ConstraintPersistentData) cp.userPersistentData;
                if (cpd != null) {
                    // might be invalid
                    cpd.persistentLifeTime++;
                    if (cpd.persistentLifeTime != cp.getLifeTime()) {
                        //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                        //new (cpd) btConstraintPersistentData;
                        cpd.reset();
                        cpd.persistentLifeTime = cp.getLifeTime();
                    } else {
                    //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                    }
                } else {
                    // todo: should this be in a pool?
                    //void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
                    //cpd = new (mem)btConstraintPersistentData;
                    cpd = new ConstraintPersistentData();
                    //assert(cpd != null);
                    totalCpd++;
                    //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
                    cp.userPersistentData = cpd;
                    cpd.persistentLifeTime = cp.getLifeTime();
                //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
                }
                assert (cpd != null);
                cpd.jacDiagABInv = 1f / jacDiagAB;
                // Dependent on Rigidbody A and B types, fetch the contact/friction response func
                // perhaps do a similar thing for friction/restutution combiner funcs...
                cpd.frictionSolverFunc = frictionDispatch[body0.frictionSolverType][body1.frictionSolverType];
                cpd.contactSolverFunc = contactDispatch[body0.contactSolverType][body1.contactSolverType];
                body0.getVelocityInLocalPoint(rel_pos1, vel1);
                body1.getVelocityInLocalPoint(rel_pos2, vel2);
                vel.sub(vel1, vel2);
                float rel_vel;
                rel_vel = cp.normalWorldOnB.dot(vel);
                float combinedRestitution = cp.combinedRestitution;
                ///btScalar(info.m_numIterations);
                cpd.penetration = cp.getDistance();
                cpd.friction = cp.combinedFriction;
                cpd.restitution = restitutionCurve(rel_vel, combinedRestitution);
                if (cpd.restitution <= 0f) {
                    cpd.restitution = 0f;
                }
                // restitution and penetration work in same direction so
                // rel_vel 
                float penVel = -cpd.penetration / info.timeStep;
                if (cpd.restitution > penVel) {
                    cpd.penetration = 0f;
                }
                float relaxation = info.damping;
                if ((info.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) {
                    cpd.appliedImpulse *= relaxation;
                } else {
                    cpd.appliedImpulse = 0f;
                }
                // for friction
                cpd.prevAppliedImpulse = cpd.appliedImpulse;
                // re-calculate friction direction every frame, todo: check if this is really needed
                TransformUtil.planeSpace1(cp.normalWorldOnB, cpd.frictionWorldTangential0, cpd.frictionWorldTangential1);
                //#define NO_FRICTION_WARMSTART 1
                //#ifdef NO_FRICTION_WARMSTART
                cpd.accumulatedTangentImpulse0 = 0f;
                cpd.accumulatedTangentImpulse1 = 0f;
                //#endif //NO_FRICTION_WARMSTART
                float denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential0);
                float denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential0);
                float denom = relaxation / (denom0 + denom1);
                cpd.jacDiagABInvTangent0 = denom;
                denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential1);
                denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential1);
                denom = relaxation / (denom0 + denom1);
                cpd.jacDiagABInvTangent1 = denom;
                //btVector3 totalImpulse = 
                //	//#ifndef NO_FRICTION_WARMSTART
                //	//cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
                //	//cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
                //	//#endif //NO_FRICTION_WARMSTART
                //	cp.normalWorldOnB*cpd.appliedImpulse;
                totalImpulse.scale(cpd.appliedImpulse, cp.normalWorldOnB);
                ///
                {
                    torqueAxis0.cross(rel_pos1, cp.normalWorldOnB);
                    cpd.angularComponentA.set(torqueAxis0);
                    body0.getInvInertiaTensorWorld(tmpMat3).transform(cpd.angularComponentA);
                    torqueAxis1.cross(rel_pos2, cp.normalWorldOnB);
                    cpd.angularComponentB.set(torqueAxis1);
                    body1.getInvInertiaTensorWorld(tmpMat3).transform(cpd.angularComponentB);
                }
                {
                    ftorqueAxis0.cross(rel_pos1, cpd.frictionWorldTangential0);
                    cpd.frictionAngularComponent0A.set(ftorqueAxis0);
                    body0.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent0A);
                }
                {
                    ftorqueAxis1.cross(rel_pos1, cpd.frictionWorldTangential1);
                    cpd.frictionAngularComponent1A.set(ftorqueAxis1);
                    body0.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent1A);
                }
                {
                    ftorqueAxis0.cross(rel_pos2, cpd.frictionWorldTangential0);
                    cpd.frictionAngularComponent0B.set(ftorqueAxis0);
                    body1.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent0B);
                }
                {
                    ftorqueAxis1.cross(rel_pos2, cpd.frictionWorldTangential1);
                    cpd.frictionAngularComponent1B.set(ftorqueAxis1);
                    body1.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent1B);
                }
                ///
                // apply previous frames impulse on both bodies
                body0.applyImpulse(totalImpulse, rel_pos1);
                tmpVec.negate(totalImpulse);
                body1.applyImpulse(tmpVec, rel_pos2);
            }
        }
    }
    stack.leave();
}

Example 27 with Matrix3f

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

the class SequentialImpulseConstraintSolver method addFrictionConstraint.

@StaticAlloc
protected void addFrictionConstraint(Vector3f normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, ManifoldPoint cp, Vector3f rel_pos1, Vector3f rel_pos2, CollisionObject colObj0, CollisionObject colObj1, float relaxation) {
    RigidBody body0 = RigidBody.upcast(colObj0);
    RigidBody body1 = RigidBody.upcast(colObj1);
    SolverConstraint solverConstraint = constraintsPool.get();
    tmpSolverFrictionConstraintPool.add(solverConstraint);
    solverConstraint.contactNormal.set(normalAxis);
    solverConstraint.solverBodyIdA = solverBodyIdA;
    solverConstraint.solverBodyIdB = solverBodyIdB;
    solverConstraint.constraintType = SolverConstraintType.SOLVER_FRICTION_1D;
    solverConstraint.frictionIndex = frictionIndex;
    solverConstraint.friction = cp.combinedFriction;
    solverConstraint.originalContactPoint = null;
    solverConstraint.appliedImpulse = 0f;
    solverConstraint.appliedPushImpulse = 0f;
    solverConstraint.penetration = 0f;
    Stack stack = Stack.enter();
    Vector3f ftorqueAxis1 = stack.allocVector3f();
    Matrix3f tmpMat = stack.allocMatrix3f();
    {
        ftorqueAxis1.cross(rel_pos1, solverConstraint.contactNormal);
        solverConstraint.relpos1CrossNormal.set(ftorqueAxis1);
        if (body0 != null) {
            solverConstraint.angularComponentA.set(ftorqueAxis1);
            body0.getInvInertiaTensorWorld(tmpMat).transform(solverConstraint.angularComponentA);
        } else {
            solverConstraint.angularComponentA.set(0f, 0f, 0f);
        }
    }
    {
        ftorqueAxis1.cross(rel_pos2, solverConstraint.contactNormal);
        solverConstraint.relpos2CrossNormal.set(ftorqueAxis1);
        if (body1 != null) {
            solverConstraint.angularComponentB.set(ftorqueAxis1);
            body1.getInvInertiaTensorWorld(tmpMat).transform(solverConstraint.angularComponentB);
        } else {
            solverConstraint.angularComponentB.set(0f, 0f, 0f);
        }
    }
    //#ifdef COMPUTE_IMPULSE_DENOM
    //	btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
    //	btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
    //#else
    Vector3f vec = stack.allocVector3f();
    float denom0 = 0f;
    float denom1 = 0f;
    if (body0 != null) {
        vec.cross(solverConstraint.angularComponentA, rel_pos1);
        denom0 = body0.getInvMass() + normalAxis.dot(vec);
    }
    if (body1 != null) {
        vec.cross(solverConstraint.angularComponentB, rel_pos2);
        denom1 = body1.getInvMass() + normalAxis.dot(vec);
    }
    //#endif //COMPUTE_IMPULSE_DENOM
    float denom = relaxation / (denom0 + denom1);
    solverConstraint.jacDiagABInv = denom;
    stack.leave();
}

Example 28 with Matrix3f

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

the class SliderConstraint method buildJacobianInt.

// internal
public void buildJacobianInt(RigidBody rbA, RigidBody rbB, Transform frameInA, Transform frameInB) {
    Stack stack = Stack.enter();
    Transform tmpTrans = stack.allocTransform();
    Transform tmpTrans1 = stack.allocTransform();
    Transform tmpTrans2 = stack.allocTransform();
    Vector3f tmp = stack.allocVector3f();
    Vector3f tmp2 = stack.allocVector3f();
    // calculate transforms
    calculatedTransformA.mul(rbA.getCenterOfMassTransform(tmpTrans), frameInA);
    calculatedTransformB.mul(rbB.getCenterOfMassTransform(tmpTrans), frameInB);
    realPivotAInW.set(calculatedTransformA.origin);
    realPivotBInW.set(calculatedTransformB.origin);
    calculatedTransformA.basis.getColumn(0, tmp);
    // along X
    sliderAxis.set(tmp);
    delta.sub(realPivotBInW, realPivotAInW);
    projPivotInW.scaleAdd(sliderAxis.dot(delta), sliderAxis, realPivotAInW);
    relPosA.sub(projPivotInW, rbA.getCenterOfMassPosition(tmp));
    relPosB.sub(realPivotBInW, rbB.getCenterOfMassPosition(tmp));
    Vector3f normalWorld = stack.allocVector3f();
    // linear part
    for (int i = 0; i < 3; i++) {
        calculatedTransformA.basis.getColumn(i, normalWorld);
        Matrix3f mat1 = rbA.getCenterOfMassTransform(tmpTrans1).basis;
        mat1.transpose();
        Matrix3f mat2 = rbB.getCenterOfMassTransform(tmpTrans2).basis;
        mat2.transpose();
        jacLin[i].init(mat1, mat2, relPosA, relPosB, normalWorld, rbA.getInvInertiaDiagLocal(tmp), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(tmp2), rbB.getInvMass());
        jacLinDiagABInv[i] = 1f / jacLin[i].getDiagonal();
        VectorUtil.setCoord(depth, i, delta.dot(normalWorld));
    }
    testLinLimits();
    // angular part
    for (int i = 0; i < 3; i++) {
        calculatedTransformA.basis.getColumn(i, normalWorld);
        Matrix3f mat1 = rbA.getCenterOfMassTransform(tmpTrans1).basis;
        mat1.transpose();
        Matrix3f mat2 = rbB.getCenterOfMassTransform(tmpTrans2).basis;
        mat2.transpose();
        jacAng[i].init(normalWorld, mat1, mat2, rbA.getInvInertiaDiagLocal(tmp), rbB.getInvInertiaDiagLocal(tmp2));
    }
    testAngLimits();
    Vector3f axisA = stack.allocVector3f();
    calculatedTransformA.basis.getColumn(0, axisA);
    kAngle = 1f / (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
    // clear accumulator for motors
    accumulatedLinMotorImpulse = 0f;
    accumulatedAngMotorImpulse = 0f;
    stack.leave();
}

Example 29 with Matrix3f

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

the class RaycastVehicle method updateFriction.

public void updateFriction(float timeStep) {
    // calculate the impulse, so that the wheels don't move sidewards
    int numWheel = getNumWheels();
    if (numWheel == 0) {
        return;
    }
    MiscUtil.resize(forwardWS, numWheel, Suppliers.NEW_VECTOR3F_SUPPLIER);
    MiscUtil.resize(axle, numWheel, Suppliers.NEW_VECTOR3F_SUPPLIER);
    MiscUtil.resize(forwardImpulse, numWheel, 0f);
    MiscUtil.resize(sideImpulse, numWheel, 0f);
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    int numWheelsOnGround = 0;
    // collapse all those loops into one!
    for (int i = 0; i < getNumWheels(); i++) {
        WheelInfo wheel_info = wheelInfo.getQuick(i);
        RigidBody groundObject = (RigidBody) wheel_info.raycastInfo.groundObject;
        if (groundObject != null) {
            numWheelsOnGround++;
        }
        sideImpulse.set(i, 0f);
        forwardImpulse.set(i, 0f);
    }
    {
        Transform wheelTrans = stack.allocTransform();
        for (int i = 0; i < getNumWheels(); i++) {
            WheelInfo wheel_info = wheelInfo.getQuick(i);
            RigidBody groundObject = (RigidBody) wheel_info.raycastInfo.groundObject;
            if (groundObject != null) {
                getWheelTransformWS(i, wheelTrans);
                Matrix3f wheelBasis0 = stack.alloc(wheelTrans.basis);
                axle.getQuick(i).set(wheelBasis0.getElement(0, indexRightAxis), wheelBasis0.getElement(1, indexRightAxis), wheelBasis0.getElement(2, indexRightAxis));
                Vector3f surfNormalWS = wheel_info.raycastInfo.contactNormalWS;
                float proj = axle.getQuick(i).dot(surfNormalWS);
                tmp.scale(proj, surfNormalWS);
                axle.getQuick(i).sub(tmp);
                axle.getQuick(i).normalize();
                forwardWS.getQuick(i).cross(surfNormalWS, axle.getQuick(i));
                forwardWS.getQuick(i).normalize();
                float[] floatPtr = floatArrays.getFixed(1);
                ContactConstraint.resolveSingleBilateral(chassisBody, wheel_info.raycastInfo.contactPointWS, groundObject, wheel_info.raycastInfo.contactPointWS, 0f, axle.getQuick(i), floatPtr, timeStep);
                sideImpulse.set(i, floatPtr[0]);
                floatArrays.release(floatPtr);
                sideImpulse.set(i, sideImpulse.get(i) * sideFrictionStiffness2);
            }
        }
    }
    float sideFactor = 1f;
    float fwdFactor = 0.5f;
    boolean sliding = false;
    {
        for (int wheel = 0; wheel < getNumWheels(); wheel++) {
            WheelInfo wheel_info = wheelInfo.getQuick(wheel);
            RigidBody groundObject = (RigidBody) wheel_info.raycastInfo.groundObject;
            float rollingFriction = 0f;
            if (groundObject != null) {
                if (wheel_info.engineForce != 0f) {
                    rollingFriction = wheel_info.engineForce * timeStep;
                } else {
                    float defaultRollingFrictionImpulse = 0f;
                    float maxImpulse = wheel_info.brake != 0f ? wheel_info.brake : defaultRollingFrictionImpulse;
                    WheelContactPoint contactPt = new WheelContactPoint(chassisBody, groundObject, wheel_info.raycastInfo.contactPointWS, forwardWS.getQuick(wheel), maxImpulse);
                    rollingFriction = calcRollingFriction(contactPt);
                }
            }
            // switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
            forwardImpulse.set(wheel, 0f);
            wheelInfo.getQuick(wheel).skidInfo = 1f;
            if (groundObject != null) {
                wheelInfo.getQuick(wheel).skidInfo = 1f;
                float maximp = wheel_info.wheelsSuspensionForce * timeStep * wheel_info.frictionSlip;
                float maximpSide = maximp;
                float maximpSquared = maximp * maximpSide;
                //wheelInfo.m_engineForce* timeStep;
                forwardImpulse.set(wheel, rollingFriction);
                float x = (forwardImpulse.get(wheel)) * fwdFactor;
                float y = (sideImpulse.get(wheel)) * sideFactor;
                float impulseSquared = (x * x + y * y);
                if (impulseSquared > maximpSquared) {
                    sliding = true;
                    float factor = maximp / (float) Math.sqrt(impulseSquared);
                    wheelInfo.getQuick(wheel).skidInfo *= factor;
                }
            }
        }
    }
    if (sliding) {
        for (int wheel = 0; wheel < getNumWheels(); wheel++) {
            if (sideImpulse.get(wheel) != 0f) {
                if (wheelInfo.getQuick(wheel).skidInfo < 1f) {
                    forwardImpulse.set(wheel, forwardImpulse.get(wheel) * wheelInfo.getQuick(wheel).skidInfo);
                    sideImpulse.set(wheel, sideImpulse.get(wheel) * wheelInfo.getQuick(wheel).skidInfo);
                }
            }
        }
    }
    // apply the impulses
    {
        for (int wheel = 0; wheel < getNumWheels(); wheel++) {
            WheelInfo wheel_info = wheelInfo.getQuick(wheel);
            Vector3f rel_pos = stack.allocVector3f();
            rel_pos.sub(wheel_info.raycastInfo.contactPointWS, chassisBody.getCenterOfMassPosition(tmp));
            if (forwardImpulse.get(wheel) != 0f) {
                tmp.scale(forwardImpulse.get(wheel), forwardWS.getQuick(wheel));
                chassisBody.applyImpulse(tmp, rel_pos);
            }
            if (sideImpulse.get(wheel) != 0f) {
                RigidBody groundObject = (RigidBody) wheelInfo.getQuick(wheel).raycastInfo.groundObject;
                Vector3f rel_pos2 = stack.allocVector3f();
                rel_pos2.sub(wheel_info.raycastInfo.contactPointWS, groundObject.getCenterOfMassPosition(tmp));
                Vector3f sideImp = stack.allocVector3f();
                sideImp.scale(sideImpulse.get(wheel), axle.getQuick(wheel));
                rel_pos.z *= wheel_info.rollInfluence;
                chassisBody.applyImpulse(sideImp, rel_pos);
                // apply friction impulse on the ground
                tmp.negate(sideImp);
                groundObject.applyImpulse(tmp, rel_pos2);
            }
        }
    }
    stack.leave();
}

Example 30 with Matrix3f

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

the class AabbUtil2 method transformAabb.

public static void transformAabb(Vector3f halfExtents, float margin, Transform t, Vector3f aabbMinOut, Vector3f aabbMaxOut) {
    Stack stack = Stack.enter();
    Vector3f halfExtentsWithMargin = stack.allocVector3f();
    halfExtentsWithMargin.x = halfExtents.x + margin;
    halfExtentsWithMargin.y = halfExtents.y + margin;
    halfExtentsWithMargin.z = halfExtents.z + margin;
    Matrix3f abs_b = stack.alloc(t.basis);
    MatrixUtil.absolute(abs_b);
    Vector3f tmp = stack.allocVector3f();
    Vector3f center = stack.alloc(t.origin);
    Vector3f extent = stack.allocVector3f();
    abs_b.getRow(0, tmp);
    extent.x = tmp.dot(halfExtentsWithMargin);
    abs_b.getRow(1, tmp);
    extent.y = tmp.dot(halfExtentsWithMargin);
    abs_b.getRow(2, tmp);
    extent.z = tmp.dot(halfExtentsWithMargin);
    aabbMinOut.sub(center, extent);
    aabbMaxOut.add(center, extent);
    stack.leave();
}