@Override
public void buildJacobian() {
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Transform tmpTrans = Stack.alloc(Transform.class);
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.alloc(Vector3f.class);
relPos.sub(pivotBInW, pivotAInW);
// TODO: stack
Vector3f[] normal /*[3]*/ =
new Vector3f[] {
Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class)
};
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.alloc(Transform.class)).basis;
mat1.transpose();
Matrix3f mat2 = rbB.getCenterOfMassTransform(Stack.alloc(Transform.class)).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.alloc(Vector3f.class)),
rbA.getInvMass(),
rbB.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)),
rbB.getInvMass());
}
}
Vector3f b1Axis1 = Stack.alloc(Vector3f.class),
b1Axis2 = Stack.alloc(Vector3f.class),
b1Axis3 = Stack.alloc(Vector3f.class);
Vector3f b2Axis1 = Stack.alloc(Vector3f.class), b2Axis2 = Stack.alloc(Vector3f.class);
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.alloc(Vector3f.class);
rbBFrame.basis.getColumn(1, b2Axis2);
getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis2);
Quat4f rotationArc =
QuaternionUtil.shortestArcQuat(b2Axis1, b1Axis1, Stack.alloc(Quat4f.class));
Vector3f TwistRef =
QuaternionUtil.quatRotate(rotationArc, b2Axis2, Stack.alloc(Vector3f.class));
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));
}
}
}
