/*
|
* Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
|
*
|
* Bullet Continuous Collision Detection and Physics Library
|
* btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
|
*
|
* This software is provided 'as-is', without any express or implied warranty.
|
* In no event will the authors be held liable for any damages arising from
|
* the use of this software.
|
*
|
* Permission is granted to anyone to use this software for any purpose,
|
* including commercial applications, and to alter it and redistribute it
|
* freely, subject to the following restrictions:
|
*
|
* 1. The origin of this software must not be misrepresented; you must not
|
* claim that you wrote the original software. If you use this software
|
* in a product, an acknowledgment in the product documentation would be
|
* appreciated but is not required.
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
* misrepresented as being the original software.
|
* 3. This notice may not be removed or altered from any source distribution.
|
*
|
* Written by: Marcus Hennix
|
*/
|
|
package com.bulletphysics.dynamics.constraintsolver;
|
|
import com.bulletphysics.BulletGlobals;
|
import com.bulletphysics.dynamics.RigidBody;
|
import com.bulletphysics.linearmath.QuaternionUtil;
|
import com.bulletphysics.linearmath.ScalarUtil;
|
import com.bulletphysics.linearmath.Transform;
|
import com.bulletphysics.linearmath.TransformUtil;
|
import cz.advel.stack.Stack;
|
import javax.vecmath.Matrix3f;
|
import javax.vecmath.Quat4f;
|
import javax.vecmath.Vector3f;
|
|
/**
|
* ConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
|
*
|
* @author jezek2
|
*/
|
public class ConeTwistConstraint extends TypedConstraint {
|
|
private JacobianEntry[] jac/*[3]*/ = new JacobianEntry[] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; //3 orthogonal linear constraints
|
|
private final Transform rbAFrame = new Transform();
|
private final Transform rbBFrame = new Transform();
|
|
private float limitSoftness;
|
private float biasFactor;
|
private float relaxationFactor;
|
|
private float swingSpan1;
|
private float swingSpan2;
|
private float twistSpan;
|
|
private final Vector3f swingAxis = new Vector3f();
|
private final Vector3f twistAxis = new Vector3f();
|
|
private float kSwing;
|
private float kTwist;
|
|
private float twistLimitSign;
|
private float swingCorrection;
|
private float twistCorrection;
|
|
private float accSwingLimitImpulse;
|
private float accTwistLimitImpulse;
|
|
private boolean angularOnly = false;
|
private boolean solveTwistLimit;
|
private boolean solveSwingLimit;
|
|
public ConeTwistConstraint() {
|
super(TypedConstraintType.CONETWIST_CONSTRAINT_TYPE);
|
}
|
|
public ConeTwistConstraint(RigidBody rbA, RigidBody rbB, Transform rbAFrame, Transform rbBFrame) {
|
super(TypedConstraintType.CONETWIST_CONSTRAINT_TYPE, rbA, rbB);
|
this.rbAFrame.set(rbAFrame);
|
this.rbBFrame.set(rbBFrame);
|
|
swingSpan1 = 1e30f;
|
swingSpan2 = 1e30f;
|
twistSpan = 1e30f;
|
biasFactor = 0.3f;
|
relaxationFactor = 1.0f;
|
|
solveTwistLimit = false;
|
solveSwingLimit = false;
|
}
|
|
public ConeTwistConstraint(RigidBody rbA, Transform rbAFrame) {
|
super(TypedConstraintType.CONETWIST_CONSTRAINT_TYPE, rbA);
|
this.rbAFrame.set(rbAFrame);
|
this.rbBFrame.set(this.rbAFrame);
|
|
swingSpan1 = 1e30f;
|
swingSpan2 = 1e30f;
|
twistSpan = 1e30f;
|
biasFactor = 0.3f;
|
relaxationFactor = 1.0f;
|
|
solveTwistLimit = false;
|
solveSwingLimit = false;
|
}
|
|
@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 = (Vector3f) Stack.alloc(rbAFrame.origin);
|
rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);
|
|
Vector3f pivotBInW = (Vector3f) 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));
|
}
|
}
|
}
|
|
@Override
|
public void solveConstraint(float timeStep) {
|
Vector3f tmp = Stack.alloc(Vector3f.class);
|
Vector3f tmp2 = Stack.alloc(Vector3f.class);
|
|
Vector3f tmpVec = Stack.alloc(Vector3f.class);
|
Transform tmpTrans = Stack.alloc(Transform.class);
|
|
Vector3f pivotAInW = (Vector3f) Stack.alloc(rbAFrame.origin);
|
rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);
|
|
Vector3f pivotBInW = (Vector3f) Stack.alloc(rbBFrame.origin);
|
rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);
|
|
float tau = 0.3f;
|
|
// linear part
|
if (!angularOnly) {
|
Vector3f rel_pos1 = Stack.alloc(Vector3f.class);
|
rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
|
|
Vector3f rel_pos2 = Stack.alloc(Vector3f.class);
|
rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
|
|
Vector3f vel1 = rbA.getVelocityInLocalPoint(rel_pos1, Stack.alloc(Vector3f.class));
|
Vector3f vel2 = rbB.getVelocityInLocalPoint(rel_pos2, Stack.alloc(Vector3f.class));
|
Vector3f vel = Stack.alloc(Vector3f.class);
|
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);
|
float depth = -(tmp).dot(normal); // this is the error projected on the normal
|
float impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
|
appliedImpulse += impulse;
|
Vector3f impulse_vector = Stack.alloc(Vector3f.class);
|
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.alloc(Vector3f.class));
|
Vector3f angVelB = getRigidBodyB().getAngularVelocity(Stack.alloc(Vector3f.class));
|
|
// 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.alloc(Vector3f.class);
|
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.alloc(Vector3f.class);
|
impulse.scale(impulseMag, twistAxis);
|
|
rbA.applyTorqueImpulse(impulse);
|
|
tmp.negate(impulse);
|
rbB.applyTorqueImpulse(tmp);
|
}
|
}
|
}
|
|
public void updateRHS(float timeStep) {
|
}
|
|
public void setAngularOnly(boolean angularOnly) {
|
this.angularOnly = angularOnly;
|
}
|
|
public void setLimit(float _swingSpan1, float _swingSpan2, float _twistSpan) {
|
setLimit(_swingSpan1, _swingSpan2, _twistSpan, 0.8f, 0.3f, 1.0f);
|
}
|
|
public void setLimit(float _swingSpan1, float _swingSpan2, float _twistSpan, float _softness, float _biasFactor, float _relaxationFactor) {
|
swingSpan1 = _swingSpan1;
|
swingSpan2 = _swingSpan2;
|
twistSpan = _twistSpan;
|
|
limitSoftness = _softness;
|
biasFactor = _biasFactor;
|
relaxationFactor = _relaxationFactor;
|
}
|
|
public Transform getAFrame(Transform out) {
|
out.set(rbAFrame);
|
return out;
|
}
|
|
public Transform getBFrame(Transform out) {
|
out.set(rbBFrame);
|
return out;
|
}
|
|
public boolean getSolveTwistLimit() {
|
return solveTwistLimit;
|
}
|
|
public boolean getSolveSwingLimit() {
|
return solveTwistLimit;
|
}
|
|
public float getTwistLimitSign() {
|
return twistLimitSign;
|
}
|
|
}
|
