Examples with Stack com.bulletphysics.util.Stack used on opensource projects

Example 91 with Stack

use of com.bulletphysics.util.Stack 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 92 with Stack

use of com.bulletphysics.util.Stack 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 93 with Stack

use of com.bulletphysics.util.Stack 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();
}

Example 94 with Stack

use of com.bulletphysics.util.Stack in project bdx by GoranM.

the class SolverBody method getVelocityInLocalPoint.

public void getVelocityInLocalPoint(Vector3f rel_pos, Vector3f velocity) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    tmp.cross(angularVelocity, rel_pos);
    velocity.add(linearVelocity, tmp);
    stack.leave();
}

Example 95 with Stack

use of com.bulletphysics.util.Stack in project bdx by GoranM.

the class RaycastVehicle method updateWheelTransform.

public void updateWheelTransform(int wheelIndex, boolean interpolatedTransform) {
    WheelInfo wheel = wheelInfo.getQuick(wheelIndex);
    updateWheelTransformsWS(wheel, interpolatedTransform);
    Stack stack = Stack.enter();
    Vector3f up = stack.allocVector3f();
    up.negate(wheel.raycastInfo.wheelDirectionWS);
    Vector3f right = wheel.raycastInfo.wheelAxleWS;
    Vector3f fwd = stack.allocVector3f();
    fwd.cross(up, right);
    fwd.normalize();
    // up = right.cross(fwd);
    // up.normalize();
    // rotate around steering over de wheelAxleWS
    float steering = wheel.steering;
    Quat4f steeringOrn = stack.allocQuat4f();
    //wheel.m_steering);
    QuaternionUtil.setRotation(steeringOrn, up, steering);
    Matrix3f steeringMat = stack.allocMatrix3f();
    MatrixUtil.setRotation(steeringMat, steeringOrn);
    Quat4f rotatingOrn = stack.allocQuat4f();
    QuaternionUtil.setRotation(rotatingOrn, right, -wheel.rotation);
    Matrix3f rotatingMat = stack.allocMatrix3f();
    MatrixUtil.setRotation(rotatingMat, rotatingOrn);
    Matrix3f basis2 = stack.allocMatrix3f();
    basis2.setRow(0, right.x, fwd.x, up.x);
    basis2.setRow(1, right.y, fwd.y, up.y);
    basis2.setRow(2, right.z, fwd.z, up.z);
    Matrix3f wheelBasis = wheel.worldTransform.basis;
    wheelBasis.mul(steeringMat, rotatingMat);
    wheelBasis.mul(basis2);
    wheel.worldTransform.origin.scaleAdd(wheel.raycastInfo.suspensionLength, wheel.raycastInfo.wheelDirectionWS, wheel.raycastInfo.hardPointWS);
    stack.leave();
}