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

Example 81 with Vector3f

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

the class SequentialImpulseConstraintSolver method resolveSplitPenetrationImpulseCacheFriendly.

private void resolveSplitPenetrationImpulseCacheFriendly(SolverBody body1, SolverBody body2, SolverConstraint contactConstraint, ContactSolverInfo solverInfo) {
    if (contactConstraint.penetration < solverInfo.splitImpulsePenetrationThreshold) {
        BulletStats.gNumSplitImpulseRecoveries++;
        float normalImpulse;
        //  Optimized version of projected relative velocity, use precomputed cross products with normal
        //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
        //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
        //      btVector3 vel = vel1 - vel2;
        //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
        float rel_vel;
        float vel1Dotn = contactConstraint.contactNormal.dot(body1.pushVelocity) + contactConstraint.relpos1CrossNormal.dot(body1.turnVelocity);
        float vel2Dotn = contactConstraint.contactNormal.dot(body2.pushVelocity) + contactConstraint.relpos2CrossNormal.dot(body2.turnVelocity);
        rel_vel = vel1Dotn - vel2Dotn;
        float positionalError = -contactConstraint.penetration * solverInfo.erp2 / solverInfo.timeStep;
        //      btScalar positionalError = contactConstraint.m_penetration;
        // * damping;
        float velocityError = contactConstraint.restitution - rel_vel;
        float penetrationImpulse = positionalError * contactConstraint.jacDiagABInv;
        float velocityImpulse = velocityError * contactConstraint.jacDiagABInv;
        normalImpulse = penetrationImpulse + velocityImpulse;
        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
        float oldNormalImpulse = contactConstraint.appliedPushImpulse;
        float sum = oldNormalImpulse + normalImpulse;
        contactConstraint.appliedPushImpulse = 0f > sum ? 0f : sum;
        normalImpulse = contactConstraint.appliedPushImpulse - oldNormalImpulse;
        Stack stack = Stack.enter();
        Vector3f tmp = stack.allocVector3f();
        tmp.scale(body1.invMass, contactConstraint.contactNormal);
        body1.internalApplyPushImpulse(tmp, contactConstraint.angularComponentA, normalImpulse);
        tmp.scale(body2.invMass, contactConstraint.contactNormal);
        body2.internalApplyPushImpulse(tmp, contactConstraint.angularComponentB, -normalImpulse);
        stack.leave();
    }
}

Example 82 with Vector3f

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

the class SliderConstraint method testAngLimits.

public void testAngLimits() {
    angDepth = 0f;
    solveAngLim = false;
    if (lowerAngLimit <= upperAngLimit) {
        Stack stack = Stack.enter();
        Vector3f axisA0 = stack.allocVector3f();
        calculatedTransformA.basis.getColumn(1, axisA0);
        Vector3f axisA1 = stack.allocVector3f();
        calculatedTransformA.basis.getColumn(2, axisA1);
        Vector3f axisB0 = stack.allocVector3f();
        calculatedTransformB.basis.getColumn(1, axisB0);
        float rot = (float) Math.atan2(axisB0.dot(axisA1), axisB0.dot(axisA0));
        if (rot < lowerAngLimit) {
            angDepth = rot - lowerAngLimit;
            solveAngLim = true;
        } else if (rot > upperAngLimit) {
            angDepth = rot - upperAngLimit;
            solveAngLim = true;
        }
        stack.leave();
    }
}

Example 83 with Vector3f

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

the class SliderConstraint method solveConstraintInt.

public void solveConstraintInt(RigidBody rbA, RigidBody rbB) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    // linear
    Vector3f velA = rbA.getVelocityInLocalPoint(relPosA, stack.allocVector3f());
    Vector3f velB = rbB.getVelocityInLocalPoint(relPosB, stack.allocVector3f());
    Vector3f vel = stack.allocVector3f();
    vel.sub(velA, velB);
    Vector3f impulse_vector = stack.allocVector3f();
    for (int i = 0; i < 3; i++) {
        Vector3f normal = jacLin[i].linearJointAxis;
        float rel_vel = normal.dot(vel);
        // calculate positional error
        float depth = VectorUtil.getCoord(this.depth, i);
        // get parameters
        float softness = (i != 0) ? softnessOrthoLin : (solveLinLim ? softnessLimLin : softnessDirLin);
        float restitution = (i != 0) ? restitutionOrthoLin : (solveLinLim ? restitutionLimLin : restitutionDirLin);
        float damping = (i != 0) ? dampingOrthoLin : (solveLinLim ? dampingLimLin : dampingDirLin);
        // calcutate and apply impulse
        float normalImpulse = softness * (restitution * depth / timeStep - damping * rel_vel) * jacLinDiagABInv[i];
        impulse_vector.scale(normalImpulse, normal);
        rbA.applyImpulse(impulse_vector, relPosA);
        tmp.negate(impulse_vector);
        rbB.applyImpulse(tmp, relPosB);
        if (poweredLinMotor && (i == 0)) {
            // apply linear motor
            if (accumulatedLinMotorImpulse < maxLinMotorForce) {
                float desiredMotorVel = targetLinMotorVelocity;
                float motor_relvel = desiredMotorVel + rel_vel;
                normalImpulse = -motor_relvel * jacLinDiagABInv[i];
                // clamp accumulated impulse
                float new_acc = accumulatedLinMotorImpulse + Math.abs(normalImpulse);
                if (new_acc > maxLinMotorForce) {
                    new_acc = maxLinMotorForce;
                }
                float del = new_acc - accumulatedLinMotorImpulse;
                if (normalImpulse < 0f) {
                    normalImpulse = -del;
                } else {
                    normalImpulse = del;
                }
                accumulatedLinMotorImpulse = new_acc;
                // apply clamped impulse
                impulse_vector.scale(normalImpulse, normal);
                rbA.applyImpulse(impulse_vector, relPosA);
                tmp.negate(impulse_vector);
                rbB.applyImpulse(tmp, relPosB);
            }
        }
    }
    // angular
    // get axes in world space
    Vector3f axisA = stack.allocVector3f();
    calculatedTransformA.basis.getColumn(0, axisA);
    Vector3f axisB = stack.allocVector3f();
    calculatedTransformB.basis.getColumn(0, axisB);
    Vector3f angVelA = rbA.getAngularVelocity(stack.allocVector3f());
    Vector3f angVelB = rbB.getAngularVelocity(stack.allocVector3f());
    Vector3f angVelAroundAxisA = stack.allocVector3f();
    angVelAroundAxisA.scale(axisA.dot(angVelA), axisA);
    Vector3f angVelAroundAxisB = stack.allocVector3f();
    angVelAroundAxisB.scale(axisB.dot(angVelB), axisB);
    Vector3f angAorthog = stack.allocVector3f();
    angAorthog.sub(angVelA, angVelAroundAxisA);
    Vector3f angBorthog = stack.allocVector3f();
    angBorthog.sub(angVelB, angVelAroundAxisB);
    Vector3f velrelOrthog = stack.allocVector3f();
    velrelOrthog.sub(angAorthog, angBorthog);
    // solve orthogonal angular velocity correction
    float len = velrelOrthog.length();
    if (len > 0.00001f) {
        Vector3f normal = stack.allocVector3f();
        normal.normalize(velrelOrthog);
        float denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
        velrelOrthog.scale((1f / denom) * dampingOrthoAng * softnessOrthoAng);
    }
    // solve angular positional correction
    Vector3f angularError = stack.allocVector3f();
    angularError.cross(axisA, axisB);
    angularError.scale(1f / timeStep);
    float len2 = angularError.length();
    if (len2 > 0.00001f) {
        Vector3f normal2 = stack.allocVector3f();
        normal2.normalize(angularError);
        float denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
        angularError.scale((1f / denom2) * restitutionOrthoAng * softnessOrthoAng);
    }
    // apply impulse
    tmp.negate(velrelOrthog);
    tmp.add(angularError);
    rbA.applyTorqueImpulse(tmp);
    tmp.sub(velrelOrthog, angularError);
    rbB.applyTorqueImpulse(tmp);
    float impulseMag;
    // solve angular limits
    if (solveAngLim) {
        tmp.sub(angVelB, angVelA);
        impulseMag = tmp.dot(axisA) * dampingLimAng + angDepth * restitutionLimAng / timeStep;
        impulseMag *= kAngle * softnessLimAng;
    } else {
        tmp.sub(angVelB, angVelA);
        impulseMag = tmp.dot(axisA) * dampingDirAng + angDepth * restitutionDirAng / timeStep;
        impulseMag *= kAngle * softnessDirAng;
    }
    Vector3f impulse = stack.allocVector3f();
    impulse.scale(impulseMag, axisA);
    rbA.applyTorqueImpulse(impulse);
    tmp.negate(impulse);
    rbB.applyTorqueImpulse(tmp);
    // apply angular motor
    if (poweredAngMotor) {
        if (accumulatedAngMotorImpulse < maxAngMotorForce) {
            Vector3f velrel = stack.allocVector3f();
            velrel.sub(angVelAroundAxisA, angVelAroundAxisB);
            float projRelVel = velrel.dot(axisA);
            float desiredMotorVel = targetAngMotorVelocity;
            float motor_relvel = desiredMotorVel - projRelVel;
            float angImpulse = kAngle * motor_relvel;
            // clamp accumulated impulse
            float new_acc = accumulatedAngMotorImpulse + Math.abs(angImpulse);
            if (new_acc > maxAngMotorForce) {
                new_acc = maxAngMotorForce;
            }
            float del = new_acc - accumulatedAngMotorImpulse;
            if (angImpulse < 0f) {
                angImpulse = -del;
            } else {
                angImpulse = del;
            }
            accumulatedAngMotorImpulse = new_acc;
            // apply clamped impulse
            Vector3f motorImp = stack.allocVector3f();
            motorImp.scale(angImpulse, axisA);
            rbA.applyTorqueImpulse(motorImp);
            tmp.negate(motorImp);
            rbB.applyTorqueImpulse(tmp);
        }
    }
    stack.leave();
}

Example 84 with Vector3f

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

the class SliderConstraint method getAncorInA.

// access for PE Solver
public Vector3f getAncorInA(Vector3f out) {
    Stack stack = Stack.enter();
    Transform tmpTrans = stack.allocTransform();
    Vector3f ancorInA = out;
    ancorInA.scaleAdd((lowerLinLimit + upperLinLimit) * 0.5f, sliderAxis, realPivotAInW);
    rbA.getCenterOfMassTransform(tmpTrans);
    tmpTrans.inverse();
    tmpTrans.transform(ancorInA);
    stack.leave();
    return ancorInA;
}

Example 85 with Vector3f

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

the class SliderConstraint method calculateTransforms.

// shared code used by ODE solver
public void calculateTransforms() {
    Stack stack = Stack.enter();
    Transform tmpTrans = stack.allocTransform();
    if (useLinearReferenceFrameA) {
        calculatedTransformA.mul(rbA.getCenterOfMassTransform(tmpTrans), frameInA);
        calculatedTransformB.mul(rbB.getCenterOfMassTransform(tmpTrans), frameInB);
    } else {
        calculatedTransformA.mul(rbB.getCenterOfMassTransform(tmpTrans), frameInB);
        calculatedTransformB.mul(rbA.getCenterOfMassTransform(tmpTrans), frameInA);
    }
    realPivotAInW.set(calculatedTransformA.origin);
    realPivotBInW.set(calculatedTransformB.origin);
    // along X
    calculatedTransformA.basis.getColumn(0, sliderAxis);
    delta.sub(realPivotBInW, realPivotAInW);
    projPivotInW.scaleAdd(sliderAxis.dot(delta), sliderAxis, realPivotAInW);
    Vector3f normalWorld = stack.allocVector3f();
    // linear part
    for (int i = 0; i < 3; i++) {
        calculatedTransformA.basis.getColumn(i, normalWorld);
        VectorUtil.setCoord(depth, i, delta.dot(normalWorld));
    }
    stack.leave();
}