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

Example 76 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 77 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;
}

Example 78 with Vector3f

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

the class JacobianEntry method getRelativeVelocity.

public float getRelativeVelocity(Vector3f linvelA, Vector3f angvelA, Vector3f linvelB, Vector3f angvelB) {
    Stack stack = Stack.enter();
    Vector3f linrel = stack.allocVector3f();
    linrel.sub(linvelA, linvelB);
    Vector3f angvela = stack.allocVector3f();
    VectorUtil.mul(angvela, angvelA, aJ);
    Vector3f angvelb = stack.allocVector3f();
    VectorUtil.mul(angvelb, angvelB, bJ);
    VectorUtil.mul(linrel, linrel, linearJointAxis);
    angvela.add(angvelb);
    angvela.add(linrel);
    float rel_vel2 = angvela.x + angvela.y + angvela.z;
    stack.leave();
    return rel_vel2 + BulletGlobals.FLT_EPSILON;
}

Example 79 with Vector3f

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

the class Point2PointConstraint 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 centerOfMassA = rbA.getCenterOfMassTransform(stack.allocTransform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(stack.allocTransform());
    Vector3f pivotAInW = stack.alloc(pivotInA);
    centerOfMassA.transform(pivotAInW);
    Vector3f pivotBInW = stack.alloc(pivotInB);
    centerOfMassB.transform(pivotBInW);
    Vector3f normal = stack.allocVector3f();
    normal.set(0f, 0f, 0f);
    for (int i = 0; i < 3; i++) {
        VectorUtil.setCoord(normal, i, 1f);
        float jacDiagABInv = 1f / jac[i].getDiagonal();
        Vector3f rel_pos1 = stack.allocVector3f();
        rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
        Vector3f rel_pos2 = stack.allocVector3f();
        rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
        // this jacobian entry could be re-used for all iterations
        Vector3f vel1 = rbA.getVelocityInLocalPoint(rel_pos1, stack.allocVector3f());
        Vector3f vel2 = rbB.getVelocityInLocalPoint(rel_pos2, stack.allocVector3f());
        Vector3f vel = stack.allocVector3f();
        vel.sub(vel1, vel2);
        float rel_vel;
        rel_vel = normal.dot(vel);
        /*
			//velocity error (first order error)
			btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
			m_rbB.getLinearVelocity(),angvelB);
			 */
        // 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 * setting.tau / timeStep * jacDiagABInv - setting.damping * rel_vel * jacDiagABInv;
        float impulseClamp = setting.impulseClamp;
        if (impulseClamp > 0f) {
            if (impulse < -impulseClamp) {
                impulse = -impulseClamp;
            }
            if (impulse > impulseClamp) {
                impulse = impulseClamp;
            }
        }
        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);
        VectorUtil.setCoord(normal, i, 0f);
    }
    stack.leave();
}

Example 80 with Vector3f

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

the class Point2PointConstraint method buildJacobian.

@Override
public void buildJacobian() {
    appliedImpulse = 0f;
    Stack stack = Stack.enter();
    Vector3f normal = stack.allocVector3f();
    normal.set(0f, 0f, 0f);
    Matrix3f tmpMat1 = stack.allocMatrix3f();
    Matrix3f tmpMat2 = stack.allocMatrix3f();
    Vector3f tmp1 = stack.allocVector3f();
    Vector3f tmp2 = stack.allocVector3f();
    Vector3f tmpVec = stack.allocVector3f();
    Transform centerOfMassA = rbA.getCenterOfMassTransform(stack.allocTransform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(stack.allocTransform());
    for (int i = 0; i < 3; i++) {
        VectorUtil.setCoord(normal, i, 1f);
        tmpMat1.transpose(centerOfMassA.basis);
        tmpMat2.transpose(centerOfMassB.basis);
        tmp1.set(pivotInA);
        centerOfMassA.transform(tmp1);
        tmp1.sub(rbA.getCenterOfMassPosition(tmpVec));
        tmp2.set(pivotInB);
        centerOfMassB.transform(tmp2);
        tmp2.sub(rbB.getCenterOfMassPosition(tmpVec));
        jac[i].init(tmpMat1, tmpMat2, tmp1, tmp2, normal, rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvMass());
        VectorUtil.setCoord(normal, i, 0f);
    }
    stack.leave();
}