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

Example 71 with Vector3f

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

the class ConeTwistConstraint method solveConstraint.

@Override
public void solveConstraint(float timeStep) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Vector3f tmp2 = stack.allocVector3f();
    Vector3f tmpVec = stack.allocVector3f();
    Transform tmpTrans = stack.allocTransform();
    Vector3f pivotAInW = stack.alloc(rbAFrame.origin);
    rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);
    Vector3f pivotBInW = stack.alloc(rbBFrame.origin);
    rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);
    float tau = 0.3f;
    // linear part
    if (!angularOnly) {
        Vector3f rel_pos1 = stack.allocVector3f();
        rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
        Vector3f rel_pos2 = stack.allocVector3f();
        rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
        Vector3f vel1 = rbA.getVelocityInLocalPoint(rel_pos1, stack.allocVector3f());
        Vector3f vel2 = rbB.getVelocityInLocalPoint(rel_pos2, stack.allocVector3f());
        Vector3f vel = stack.allocVector3f();
        vel.sub(vel1, vel2);
        for (int i = 0; i < 3; i++) {
            Vector3f normal = jac[i].linearJointAxis;
            float jacDiagABInv = 1f / jac[i].getDiagonal();
            float rel_vel;
            rel_vel = normal.dot(vel);
            // positional error (zeroth order error)
            tmp.sub(pivotAInW, pivotBInW);
            // this is the error projected on the normal
            float depth = -(tmp).dot(normal);
            float impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
            appliedImpulse += impulse;
            Vector3f impulse_vector = stack.allocVector3f();
            impulse_vector.scale(impulse, normal);
            tmp.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
            rbA.applyImpulse(impulse_vector, tmp);
            tmp.negate(impulse_vector);
            tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
            rbB.applyImpulse(tmp, tmp2);
        }
    }
    {
        // solve angular part
        Vector3f angVelA = getRigidBodyA().getAngularVelocity(stack.allocVector3f());
        Vector3f angVelB = getRigidBodyB().getAngularVelocity(stack.allocVector3f());
        // solve swing limit
        if (solveSwingLimit) {
            tmp.sub(angVelB, angVelA);
            float amplitude = ((tmp).dot(swingAxis) * relaxationFactor * relaxationFactor + swingCorrection * (1f / timeStep) * biasFactor);
            float impulseMag = amplitude * kSwing;
            // Clamp the accumulated impulse
            float temp = accSwingLimitImpulse;
            accSwingLimitImpulse = Math.max(accSwingLimitImpulse + impulseMag, 0.0f);
            impulseMag = accSwingLimitImpulse - temp;
            Vector3f impulse = stack.allocVector3f();
            impulse.scale(impulseMag, swingAxis);
            rbA.applyTorqueImpulse(impulse);
            tmp.negate(impulse);
            rbB.applyTorqueImpulse(tmp);
        }
        // solve twist limit
        if (solveTwistLimit) {
            tmp.sub(angVelB, angVelA);
            float amplitude = ((tmp).dot(twistAxis) * relaxationFactor * relaxationFactor + twistCorrection * (1f / timeStep) * biasFactor);
            float impulseMag = amplitude * kTwist;
            // Clamp the accumulated impulse
            float temp = accTwistLimitImpulse;
            accTwistLimitImpulse = Math.max(accTwistLimitImpulse + impulseMag, 0.0f);
            impulseMag = accTwistLimitImpulse - temp;
            Vector3f impulse = stack.allocVector3f();
            impulse.scale(impulseMag, twistAxis);
            rbA.applyTorqueImpulse(impulse);
            tmp.negate(impulse);
            rbB.applyTorqueImpulse(tmp);
        }
    }
    stack.leave();
}

Example 72 with Vector3f

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

the class ContactConstraint method resolveSingleCollisionCombined.

/**
	 * velocity + friction<br>
	 * response between two dynamic objects with friction
	 */
public static float resolveSingleCollisionCombined(RigidBody body1, RigidBody body2, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) {
    Stack stack = Stack.enter();
    Vector3f tmpVec = stack.allocVector3f();
    Vector3f pos1 = contactPoint.getPositionWorldOnA(stack.allocVector3f());
    Vector3f pos2 = contactPoint.getPositionWorldOnB(stack.allocVector3f());
    Vector3f normal = contactPoint.normalWorldOnB;
    Vector3f rel_pos1 = stack.allocVector3f();
    rel_pos1.sub(pos1, body1.getCenterOfMassPosition(tmpVec));
    Vector3f rel_pos2 = stack.allocVector3f();
    rel_pos2.sub(pos2, body2.getCenterOfMassPosition(tmpVec));
    Vector3f vel1 = body1.getVelocityInLocalPoint(rel_pos1, stack.allocVector3f());
    Vector3f vel2 = body2.getVelocityInLocalPoint(rel_pos2, stack.allocVector3f());
    Vector3f vel = stack.allocVector3f();
    vel.sub(vel1, vel2);
    float rel_vel;
    rel_vel = normal.dot(vel);
    float Kfps = 1f / solverInfo.timeStep;
    //btScalar damping = solverInfo.m_damping ;
    float Kerp = solverInfo.erp;
    float Kcor = Kerp * Kfps;
    ConstraintPersistentData cpd = (ConstraintPersistentData) contactPoint.userPersistentData;
    assert (cpd != null);
    float distance = cpd.penetration;
    float positionalError = Kcor * -distance;
    // * damping;
    float velocityError = cpd.restitution - rel_vel;
    float penetrationImpulse = positionalError * cpd.jacDiagABInv;
    float velocityImpulse = velocityError * cpd.jacDiagABInv;
    float normalImpulse = penetrationImpulse + velocityImpulse;
    // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
    float oldNormalImpulse = cpd.appliedImpulse;
    float sum = oldNormalImpulse + normalImpulse;
    cpd.appliedImpulse = 0f > sum ? 0f : sum;
    normalImpulse = cpd.appliedImpulse - oldNormalImpulse;
    //#ifdef USE_INTERNAL_APPLY_IMPULSE
    Vector3f tmp = stack.allocVector3f();
    if (body1.getInvMass() != 0f) {
        tmp.scale(body1.getInvMass(), contactPoint.normalWorldOnB);
        body1.internalApplyImpulse(tmp, cpd.angularComponentA, normalImpulse);
    }
    if (body2.getInvMass() != 0f) {
        tmp.scale(body2.getInvMass(), contactPoint.normalWorldOnB);
        body2.internalApplyImpulse(tmp, cpd.angularComponentB, -normalImpulse);
    }
    //#else //USE_INTERNAL_APPLY_IMPULSE
    //	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
    //	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
    //#endif //USE_INTERNAL_APPLY_IMPULSE
    {
        //friction
        body1.getVelocityInLocalPoint(rel_pos1, vel1);
        body2.getVelocityInLocalPoint(rel_pos2, vel2);
        vel.sub(vel1, vel2);
        rel_vel = normal.dot(vel);
        tmp.scale(rel_vel, normal);
        Vector3f lat_vel = stack.allocVector3f();
        lat_vel.sub(vel, tmp);
        float lat_rel_vel = lat_vel.length();
        float combinedFriction = cpd.friction;
        if (cpd.appliedImpulse > 0) {
            if (lat_rel_vel > BulletGlobals.FLT_EPSILON) {
                lat_vel.scale(1f / lat_rel_vel);
                Vector3f temp1 = stack.allocVector3f();
                temp1.cross(rel_pos1, lat_vel);
                body1.getInvInertiaTensorWorld(stack.allocMatrix3f()).transform(temp1);
                Vector3f temp2 = stack.allocVector3f();
                temp2.cross(rel_pos2, lat_vel);
                body2.getInvInertiaTensorWorld(stack.allocMatrix3f()).transform(temp2);
                Vector3f java_tmp1 = stack.allocVector3f();
                java_tmp1.cross(temp1, rel_pos1);
                Vector3f java_tmp2 = stack.allocVector3f();
                java_tmp2.cross(temp2, rel_pos2);
                tmp.add(java_tmp1, java_tmp2);
                float friction_impulse = lat_rel_vel / (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(tmp));
                float normal_impulse = cpd.appliedImpulse * combinedFriction;
                friction_impulse = Math.min(friction_impulse, normal_impulse);
                friction_impulse = Math.max(friction_impulse, -normal_impulse);
                tmp.scale(-friction_impulse, lat_vel);
                body1.applyImpulse(tmp, rel_pos1);
                tmp.scale(friction_impulse, lat_vel);
                body2.applyImpulse(tmp, rel_pos2);
            }
        }
    }
    stack.leave();
    return normalImpulse;
}

Example 73 with Vector3f

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

the class Generic6DofConstraint method calculateAngleInfo.

/**
	 * Calcs the euler angles between the two bodies.
	 */
protected void calculateAngleInfo() {
    Stack stack = Stack.enter();
    Matrix3f mat = stack.allocMatrix3f();
    Matrix3f relative_frame = stack.allocMatrix3f();
    mat.set(calculatedTransformA.basis);
    MatrixUtil.invert(mat);
    relative_frame.mul(mat, calculatedTransformB.basis);
    matrixToEulerXYZ(relative_frame, calculatedAxisAngleDiff);
    // in euler angle mode we do not actually constrain the angular velocity
    // along the axes axis[0] and axis[2] (although we do use axis[1]) :
    //
    //    to get			constrain w2-w1 along		...not
    //    ------			---------------------		------
    //    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
    //    d(angle[1])/dt = 0	ax[1]
    //    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
    //
    // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
    // to prove the result for angle[0], write the expression for angle[0] from
    // GetInfo1 then take the derivative. to prove this for angle[2] it is
    // easier to take the euler rate expression for d(angle[2])/dt with respect
    // to the components of w and set that to 0.
    Vector3f axis0 = stack.allocVector3f();
    calculatedTransformB.basis.getColumn(0, axis0);
    Vector3f axis2 = stack.allocVector3f();
    calculatedTransformA.basis.getColumn(2, axis2);
    calculatedAxis[1].cross(axis2, axis0);
    calculatedAxis[0].cross(calculatedAxis[1], axis2);
    calculatedAxis[2].cross(axis0, calculatedAxis[1]);
    //    if(m_debugDrawer)
    //    {
    //
    //    	char buff[300];
    //		sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
    //		m_calculatedAxisAngleDiff[0],
    //		m_calculatedAxisAngleDiff[1],
    //		m_calculatedAxisAngleDiff[2]);
    //    	m_debugDrawer->reportErrorWarning(buff);
    //    }
    stack.leave();
}

Example 74 with Vector3f

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

the class Generic6DofConstraint method buildJacobian.

@Override
public void buildJacobian() {
    // Clear accumulated impulses for the next simulation step
    linearLimits.accumulatedImpulse.set(0f, 0f, 0f);
    for (int i = 0; i < 3; i++) {
        angularLimits[i].accumulatedImpulse = 0f;
    }
    // calculates transform
    calculateTransforms();
    Stack stack = Stack.enter();
    Vector3f tmpVec = stack.allocVector3f();
    //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
    //  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
    calcAnchorPos();
    Vector3f pivotAInW = stack.alloc(anchorPos);
    Vector3f pivotBInW = stack.alloc(anchorPos);
    // not used here
    //    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
    //    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    Vector3f normalWorld = stack.allocVector3f();
    // linear part
    for (int i = 0; i < 3; i++) {
        if (linearLimits.isLimited(i)) {
            if (useLinearReferenceFrameA) {
                calculatedTransformA.basis.getColumn(i, normalWorld);
            } else {
                calculatedTransformB.basis.getColumn(i, normalWorld);
            }
            buildLinearJacobian(/*jacLinear[i]*/
            i, normalWorld, pivotAInW, pivotBInW);
        }
    }
    // angular part
    for (int i = 0; i < 3; i++) {
        // calculates error angle
        if (testAngularLimitMotor(i)) {
            this.getAxis(i, normalWorld);
            // Create angular atom
            buildAngularJacobian(/*jacAng[i]*/
            i, normalWorld);
        }
    }
    stack.leave();
}

Example 75 with Vector3f

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

the class JacobianEntry method getNonDiagonal.

/**
	 * For two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies).
	 */
public float getNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB) {
    JacobianEntry jacA = this;
    Stack stack = Stack.enter();
    Vector3f lin = stack.allocVector3f();
    VectorUtil.mul(lin, jacA.linearJointAxis, jacB.linearJointAxis);
    Vector3f ang0 = stack.allocVector3f();
    VectorUtil.mul(ang0, jacA.m_0MinvJt, jacB.aJ);
    Vector3f ang1 = stack.allocVector3f();
    VectorUtil.mul(ang1, jacA.m_1MinvJt, jacB.bJ);
    Vector3f lin0 = stack.allocVector3f();
    lin0.scale(massInvA, lin);
    Vector3f lin1 = stack.allocVector3f();
    lin1.scale(massInvB, lin);
    Vector3f sum = stack.allocVector3f();
    VectorUtil.add(sum, ang0, ang1, lin0, lin1);
    float result = sum.x + sum.y + sum.z;
    stack.leave();
    return result;
}