Examples with Transform com.bulletphysics.linearmath.Transform used on opensource projects

Example 61 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class KinematicCharacterController method stepForwardAndStrafe.

protected void stepForwardAndStrafe(CollisionWorld collisionWorld, Vector3f walkMove) {
    // printf("m_normalizedDirection=%f,%f,%f\n",
    // 	m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
    // phase 2: forward and strafe
    Stack stack = Stack.enter();
    Transform start = stack.allocTransform();
    Transform end = stack.allocTransform();
    targetPosition.add(currentPosition, walkMove);
    start.setIdentity();
    end.setIdentity();
    float fraction = 1.0f;
    Vector3f distance2Vec = stack.allocVector3f();
    distance2Vec.sub(currentPosition, targetPosition);
    float distance2 = distance2Vec.lengthSquared();
    //printf("distance2=%f\n",distance2);
    /*if (touchingContact) {
			if (normalizedDirection.dot(touchingNormal) > 0.0f) {
				updateTargetPositionBasedOnCollision(touchingNormal);
			}
		}*/
    int maxIter = 10;
    while (fraction > 0.01f && maxIter-- > 0) {
        start.origin.set(currentPosition);
        end.origin.set(targetPosition);
        KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(ghostObject, upAxisDirection[upAxis], -1.0f);
        callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
        callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;
        float margin = convexShape.getMargin();
        convexShape.setMargin(margin + addedMargin);
        if (useGhostObjectSweepTest) {
            ghostObject.convexSweepTest(convexShape, start, end, callback, collisionWorld.getDispatchInfo().allowedCcdPenetration);
        } else {
            collisionWorld.convexSweepTest(convexShape, start, end, callback);
        }
        convexShape.setMargin(margin);
        fraction -= callback.closestHitFraction;
        if (callback.hasHit()) {
            // we moved only a fraction
            Vector3f hitDistanceVec = stack.allocVector3f();
            hitDistanceVec.sub(callback.hitPointWorld, currentPosition);
            //float hitDistance = hitDistanceVec.length();
            // if the distance is farther than the collision margin, move
            //if (hitDistance > addedMargin) {
            //	//printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
            //	currentPosition.interpolate(currentPosition, targetPosition, callback.closestHitFraction);
            //}
            updateTargetPositionBasedOnCollision(callback.hitNormalWorld);
            Vector3f currentDir = stack.allocVector3f();
            currentDir.sub(targetPosition, currentPosition);
            distance2 = currentDir.lengthSquared();
            if (distance2 > BulletGlobals.SIMD_EPSILON) {
                currentDir.normalize();
                // see Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners."
                if (currentDir.dot(normalizedDirection) <= 0.0f) {
                    break;
                }
            } else {
                //printf("currentDir: don't normalize a zero vector\n");
                break;
            }
        } else {
            // we moved whole way
            currentPosition.set(targetPosition);
        }
    //if (callback.m_closestHitFraction == 0.f)
    //    break;
    }
    stack.leave();
}

Example 62 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class ConeTwistConstraint method buildJacobian.

@Override
public void buildJacobian() {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Vector3f tmp1 = stack.allocVector3f();
    Vector3f tmp2 = stack.allocVector3f();
    Transform tmpTrans = stack.allocTransform();
    appliedImpulse = 0f;
    // set bias, sign, clear accumulator
    swingCorrection = 0f;
    twistLimitSign = 0f;
    solveTwistLimit = false;
    solveSwingLimit = false;
    accTwistLimitImpulse = 0f;
    accSwingLimitImpulse = 0f;
    if (!angularOnly) {
        Vector3f pivotAInW = stack.alloc(rbAFrame.origin);
        rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);
        Vector3f pivotBInW = stack.alloc(rbBFrame.origin);
        rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);
        Vector3f relPos = stack.allocVector3f();
        relPos.sub(pivotBInW, pivotAInW);
        // TODO: stack
        Vector3f[] normal = /*[3]*/
        new Vector3f[] { stack.allocVector3f(), stack.allocVector3f(), stack.allocVector3f() };
        if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
            normal[0].normalize(relPos);
        } else {
            normal[0].set(1f, 0f, 0f);
        }
        TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);
        for (int i = 0; i < 3; i++) {
            Matrix3f mat1 = rbA.getCenterOfMassTransform(stack.allocTransform()).basis;
            mat1.transpose();
            Matrix3f mat2 = rbB.getCenterOfMassTransform(stack.allocTransform()).basis;
            mat2.transpose();
            tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmp));
            tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmp));
            jac[i].init(mat1, mat2, tmp1, tmp2, normal[i], rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvMass());
        }
    }
    Vector3f b1Axis1 = stack.allocVector3f(), b1Axis2 = stack.allocVector3f(), b1Axis3 = stack.allocVector3f();
    Vector3f b2Axis1 = stack.allocVector3f(), b2Axis2 = stack.allocVector3f();
    rbAFrame.basis.getColumn(0, b1Axis1);
    getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis1);
    rbBFrame.basis.getColumn(0, b2Axis1);
    getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis1);
    float swing1 = 0f, swing2 = 0f;
    float swx = 0f, swy = 0f;
    float thresh = 10f;
    float fact;
    // Get Frame into world space
    if (swingSpan1 >= 0.05f) {
        rbAFrame.basis.getColumn(1, b1Axis2);
        getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis2);
        //			swing1 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis2), b2Axis1.dot(b1Axis1));
        swx = b2Axis1.dot(b1Axis1);
        swy = b2Axis1.dot(b1Axis2);
        swing1 = ScalarUtil.atan2Fast(swy, swx);
        fact = (swy * swy + swx * swx) * thresh * thresh;
        fact = fact / (fact + 1f);
        swing1 *= fact;
    }
    if (swingSpan2 >= 0.05f) {
        rbAFrame.basis.getColumn(2, b1Axis3);
        getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis3);
        //			swing2 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis3), b2Axis1.dot(b1Axis1));
        swx = b2Axis1.dot(b1Axis1);
        swy = b2Axis1.dot(b1Axis3);
        swing2 = ScalarUtil.atan2Fast(swy, swx);
        fact = (swy * swy + swx * swx) * thresh * thresh;
        fact = fact / (fact + 1f);
        swing2 *= fact;
    }
    float RMaxAngle1Sq = 1.0f / (swingSpan1 * swingSpan1);
    float RMaxAngle2Sq = 1.0f / (swingSpan2 * swingSpan2);
    float EllipseAngle = Math.abs(swing1 * swing1) * RMaxAngle1Sq + Math.abs(swing2 * swing2) * RMaxAngle2Sq;
    if (EllipseAngle > 1.0f) {
        swingCorrection = EllipseAngle - 1.0f;
        solveSwingLimit = true;
        // Calculate necessary axis & factors
        tmp1.scale(b2Axis1.dot(b1Axis2), b1Axis2);
        tmp2.scale(b2Axis1.dot(b1Axis3), b1Axis3);
        tmp.add(tmp1, tmp2);
        swingAxis.cross(b2Axis1, tmp);
        swingAxis.normalize();
        float swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
        swingAxis.scale(swingAxisSign);
        kSwing = 1f / (getRigidBodyA().computeAngularImpulseDenominator(swingAxis) + getRigidBodyB().computeAngularImpulseDenominator(swingAxis));
    }
    // Twist limits
    if (twistSpan >= 0f) {
        //Vector3f b2Axis2 = stack.allocVector3f();
        rbBFrame.basis.getColumn(1, b2Axis2);
        getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis2);
        Quat4f rotationArc = QuaternionUtil.shortestArcQuat(b2Axis1, b1Axis1, stack.allocQuat4f());
        Vector3f TwistRef = QuaternionUtil.quatRotate(rotationArc, b2Axis2, stack.allocVector3f());
        float twist = ScalarUtil.atan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
        float lockedFreeFactor = (twistSpan > 0.05f) ? limitSoftness : 0f;
        if (twist <= -twistSpan * lockedFreeFactor) {
            twistCorrection = -(twist + twistSpan);
            solveTwistLimit = true;
            twistAxis.add(b2Axis1, b1Axis1);
            twistAxis.scale(0.5f);
            twistAxis.normalize();
            twistAxis.scale(-1.0f);
            kTwist = 1f / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
        } else if (twist > twistSpan * lockedFreeFactor) {
            twistCorrection = (twist - twistSpan);
            solveTwistLimit = true;
            twistAxis.add(b2Axis1, b1Axis1);
            twistAxis.scale(0.5f);
            twistAxis.normalize();
            kTwist = 1f / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
        }
    }
    stack.leave();
}

Example 63 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class HingeConstraint method buildJacobian.

@Override
public void buildJacobian() {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Vector3f tmp1 = stack.allocVector3f();
    Vector3f tmp2 = stack.allocVector3f();
    Vector3f tmpVec = stack.allocVector3f();
    Matrix3f mat1 = stack.allocMatrix3f();
    Matrix3f mat2 = stack.allocMatrix3f();
    Transform centerOfMassA = rbA.getCenterOfMassTransform(stack.allocTransform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(stack.allocTransform());
    appliedImpulse = 0f;
    if (!angularOnly) {
        Vector3f pivotAInW = stack.alloc(rbAFrame.origin);
        centerOfMassA.transform(pivotAInW);
        Vector3f pivotBInW = stack.alloc(rbBFrame.origin);
        centerOfMassB.transform(pivotBInW);
        Vector3f relPos = stack.allocVector3f();
        relPos.sub(pivotBInW, pivotAInW);
        Vector3f[] normal = /*[3]*/
        new Vector3f[] { stack.allocVector3f(), stack.allocVector3f(), stack.allocVector3f() };
        if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
            normal[0].set(relPos);
            normal[0].normalize();
        } else {
            normal[0].set(1f, 0f, 0f);
        }
        TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);
        for (int i = 0; i < 3; i++) {
            mat1.transpose(centerOfMassA.basis);
            mat2.transpose(centerOfMassB.basis);
            tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
            tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
            jac[i].init(mat1, mat2, tmp1, tmp2, normal[i], rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvMass());
        }
    }
    // calculate two perpendicular jointAxis, orthogonal to hingeAxis
    // these two jointAxis require equal angular velocities for both bodies
    // this is unused for now, it's a todo
    Vector3f jointAxis0local = stack.allocVector3f();
    Vector3f jointAxis1local = stack.allocVector3f();
    rbAFrame.basis.getColumn(2, tmp);
    TransformUtil.planeSpace1(tmp, jointAxis0local, jointAxis1local);
    // TODO: check this
    //getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
    Vector3f jointAxis0 = stack.alloc(jointAxis0local);
    centerOfMassA.basis.transform(jointAxis0);
    Vector3f jointAxis1 = stack.alloc(jointAxis1local);
    centerOfMassA.basis.transform(jointAxis1);
    Vector3f hingeAxisWorld = stack.allocVector3f();
    rbAFrame.basis.getColumn(2, hingeAxisWorld);
    centerOfMassA.basis.transform(hingeAxisWorld);
    mat1.transpose(centerOfMassA.basis);
    mat2.transpose(centerOfMassB.basis);
    jacAng[0].init(jointAxis0, mat1, mat2, rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvInertiaDiagLocal(stack.allocVector3f()));
    // JAVA NOTE: reused mat1 and mat2, as recomputation is not needed
    jacAng[1].init(jointAxis1, mat1, mat2, rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvInertiaDiagLocal(stack.allocVector3f()));
    // JAVA NOTE: reused mat1 and mat2, as recomputation is not needed
    jacAng[2].init(hingeAxisWorld, mat1, mat2, rbA.getInvInertiaDiagLocal(stack.allocVector3f()), rbB.getInvInertiaDiagLocal(stack.allocVector3f()));
    // Compute limit information
    float hingeAngle = getHingeAngle();
    //set bias, sign, clear accumulator
    correction = 0f;
    limitSign = 0f;
    solveLimit = false;
    accLimitImpulse = 0f;
    if (lowerLimit < upperLimit) {
        if (hingeAngle <= lowerLimit * limitSoftness) {
            correction = (lowerLimit - hingeAngle);
            limitSign = 1.0f;
            solveLimit = true;
        } else if (hingeAngle >= upperLimit * limitSoftness) {
            correction = upperLimit - hingeAngle;
            limitSign = -1.0f;
            solveLimit = true;
        }
    }
    // Compute K = J*W*J' for hinge axis
    Vector3f axisA = stack.allocVector3f();
    rbAFrame.basis.getColumn(2, axisA);
    centerOfMassA.basis.transform(axisA);
    kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + getRigidBodyB().computeAngularImpulseDenominator(axisA));
    stack.leave();
}

Example 64 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class HingeConstraint method getHingeAngle.

public float getHingeAngle() {
    Stack stack = Stack.enter();
    Transform centerOfMassA = rbA.getCenterOfMassTransform(stack.allocTransform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(stack.allocTransform());
    Vector3f refAxis0 = stack.allocVector3f();
    rbAFrame.basis.getColumn(0, refAxis0);
    centerOfMassA.basis.transform(refAxis0);
    Vector3f refAxis1 = stack.allocVector3f();
    rbAFrame.basis.getColumn(1, refAxis1);
    centerOfMassA.basis.transform(refAxis1);
    Vector3f swingAxis = stack.allocVector3f();
    rbBFrame.basis.getColumn(1, swingAxis);
    centerOfMassB.basis.transform(swingAxis);
    float result = ScalarUtil.atan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
    stack.leave();
    return result;
}

Example 65 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class HingeConstraint 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(rbAFrame.origin);
    centerOfMassA.transform(pivotAInW);
    Vector3f pivotBInW = stack.alloc(rbBFrame.origin);
    centerOfMassB.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
        // get axes in world space
        Vector3f axisA = stack.allocVector3f();
        rbAFrame.basis.getColumn(2, axisA);
        centerOfMassA.basis.transform(axisA);
        Vector3f axisB = stack.allocVector3f();
        rbBFrame.basis.getColumn(2, axisB);
        centerOfMassB.basis.transform(axisB);
        Vector3f angVelA = getRigidBodyA().getAngularVelocity(stack.allocVector3f());
        Vector3f angVelB = getRigidBodyB().getAngularVelocity(stack.allocVector3f());
        Vector3f angVelAroundHingeAxisA = stack.allocVector3f();
        angVelAroundHingeAxisA.scale(axisA.dot(angVelA), axisA);
        Vector3f angVelAroundHingeAxisB = stack.allocVector3f();
        angVelAroundHingeAxisB.scale(axisB.dot(angVelB), axisB);
        Vector3f angAorthog = stack.allocVector3f();
        angAorthog.sub(angVelA, angVelAroundHingeAxisA);
        Vector3f angBorthog = stack.allocVector3f();
        angBorthog.sub(angVelB, angVelAroundHingeAxisB);
        Vector3f velrelOrthog = stack.allocVector3f();
        velrelOrthog.sub(angAorthog, angBorthog);
        {
            // solve orthogonal angular velocity correction
            float relaxation = 1f;
            float len = velrelOrthog.length();
            if (len > 0.00001f) {
                Vector3f normal = stack.allocVector3f();
                normal.normalize(velrelOrthog);
                float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + getRigidBodyB().computeAngularImpulseDenominator(normal);
                // scale for mass and relaxation
                // todo:  expose this 0.9 factor to developer
                velrelOrthog.scale((1f / denom) * relaxationFactor);
            }
            // solve angular positional correction
            // TODO: check
            //Vector3f angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
            Vector3f angularError = stack.allocVector3f();
            angularError.cross(axisA, axisB);
            angularError.negate();
            angularError.scale(1f / timeStep);
            float len2 = angularError.length();
            if (len2 > 0.00001f) {
                Vector3f normal2 = stack.allocVector3f();
                normal2.normalize(angularError);
                float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + getRigidBodyB().computeAngularImpulseDenominator(normal2);
                angularError.scale((1f / denom2) * relaxation);
            }
            tmp.negate(velrelOrthog);
            tmp.add(angularError);
            rbA.applyTorqueImpulse(tmp);
            tmp.sub(velrelOrthog, angularError);
            rbB.applyTorqueImpulse(tmp);
            // solve limit
            if (solveLimit) {
                tmp.sub(angVelB, angVelA);
                float amplitude = ((tmp).dot(axisA) * relaxationFactor + correction * (1f / timeStep) * biasFactor) * limitSign;
                float impulseMag = amplitude * kHinge;
                // Clamp the accumulated impulse
                float temp = accLimitImpulse;
                accLimitImpulse = Math.max(accLimitImpulse + impulseMag, 0f);
                impulseMag = accLimitImpulse - temp;
                Vector3f impulse = stack.allocVector3f();
                impulse.scale(impulseMag * limitSign, axisA);
                rbA.applyTorqueImpulse(impulse);
                tmp.negate(impulse);
                rbB.applyTorqueImpulse(tmp);
            }
        }
        // apply motor
        if (enableAngularMotor) {
            // todo: add limits too
            Vector3f angularLimit = stack.allocVector3f();
            angularLimit.set(0f, 0f, 0f);
            Vector3f velrel = stack.allocVector3f();
            velrel.sub(angVelAroundHingeAxisA, angVelAroundHingeAxisB);
            float projRelVel = velrel.dot(axisA);
            float desiredMotorVel = motorTargetVelocity;
            float motor_relvel = desiredMotorVel - projRelVel;
            float unclippedMotorImpulse = kHinge * motor_relvel;
            // todo: should clip against accumulated impulse
            float clippedMotorImpulse = unclippedMotorImpulse > maxMotorImpulse ? maxMotorImpulse : unclippedMotorImpulse;
            clippedMotorImpulse = clippedMotorImpulse < -maxMotorImpulse ? -maxMotorImpulse : clippedMotorImpulse;
            Vector3f motorImp = stack.allocVector3f();
            motorImp.scale(clippedMotorImpulse, axisA);
            tmp.add(motorImp, angularLimit);
            rbA.applyTorqueImpulse(tmp);
            tmp.negate(motorImp);
            tmp.sub(angularLimit);
            rbB.applyTorqueImpulse(tmp);
        }
    }
    stack.leave();
}