private CollisionShape rotate(CollisionShape shape, Quat4f rot) {
if (shape instanceof BoxShape) {
BoxShape box = (BoxShape) shape;
javax.vecmath.Vector3f extents = box.getHalfExtentsWithMargin(new javax.vecmath.Vector3f());
com.bulletphysics.linearmath.QuaternionUtil.quatRotate(VecMath.to(rot), extents, extents);
extents.absolute();
return new BoxShape(extents);
} else if (shape instanceof CompoundShape) {
CompoundShape compound = (CompoundShape) shape;
CompoundShape newShape = new CompoundShape();
for (CompoundShapeChild child : compound.getChildList()) {
CollisionShape rotatedChild = rotate(child.childShape, rot);
javax.vecmath.Vector3f offset =
com.bulletphysics.linearmath.QuaternionUtil.quatRotate(
VecMath.to(rot), child.transform.origin, new javax.vecmath.Vector3f());
newShape.addChildShape(
new Transform(
new javax.vecmath.Matrix4f(VecMath.to(Rotation.none().getQuat4f()), offset, 1.0f)),
rotatedChild);
}
return newShape;
} else if (shape instanceof ConvexHullShape) {
ConvexHullShape convexHull = (ConvexHullShape) shape;
ObjectArrayList<javax.vecmath.Vector3f> transformedVerts = new ObjectArrayList<>();
for (javax.vecmath.Vector3f vert : convexHull.getPoints()) {
transformedVerts.add(
com.bulletphysics.linearmath.QuaternionUtil.quatRotate(
VecMath.to(rot), vert, new javax.vecmath.Vector3f()));
}
return new ConvexHullShape(transformedVerts);
}
return shape;
}
private static Vector3f getEntityDirection(EntityRef entity) {
LocationComponent loc = entity.getComponent(LocationComponent.class);
if (loc != null) {
Quat4f rot = loc.getWorldRotation();
Vector3f dir = new Vector3f(0, 0, -1);
QuaternionUtil.quatRotate(rot, dir, dir);
return dir;
}
return new Vector3f();
}
public static Vector3d quatRotate(Quat4d rotation, Vector3d v, Vector3d out) {
Quat4d q = new Quat4d(rotation);
QuaternionUtil.mul(q, v);
Quat4d tmp = new Quat4d();
inverse(tmp, rotation);
q.mul(tmp);
out.set(q.x, q.y, q.z);
return out;
}
protected void doGroundMovement() {
if (!isOnGround) return;
// ground movement is as follows, the arrow key dictate the target velocity, and accelleration
// is
// added to achieve that. If opposing keys are held the target velocity is set to zero.
// This is the velocity in the normal plane in the player space
Vector3f targetVelocity = new Vector3f();
if (forward) targetVelocity.z += 1;
if (backward) targetVelocity.z -= 1;
if (left) targetVelocity.x -= 1;
if (right) targetVelocity.x += 1;
if (forward != backward || left != right) {
targetVelocity.normalize();
targetVelocity.scale(MAX_SPEED);
}
forward = false;
backward = false;
left = false;
right = false;
Vector3f forward = new Vector3f(0, 0, 1);
QuaternionUtil.quatRotate(owner.getOrientation(), forward, forward);
// right = normal x forward
// forward = right x normal
Vector3f right = new Vector3f();
right.cross(surfaceNormal, forward);
forward.cross(right, surfaceNormal);
right.normalize();
forward.normalize();
Matrix3f transform = new Matrix3f();
transform.setColumn(0, right);
transform.setColumn(1, surfaceNormal);
transform.setColumn(2, forward);
transform.transform(targetVelocity);
Vector3f delta = new Vector3f();
owner.getRigidBody().getLinearVelocity(delta);
delta.sub(targetVelocity, delta);
// feet can't pull and don't need to push for now
// Ds = D - N(N.D)
Vector3f parComp = new Vector3f(surfaceNormal);
parComp.scale(surfaceNormal.dot(delta));
delta.sub(delta, parComp);
owner.getRigidBody().applyCentralForce(scaleForce(delta));
}
public TestRig(DynamicsWorld ownerWorld, Vector3f positionOffset, boolean fixed) {
this.ownerWorld = ownerWorld;
Transform tmpTrans = new Transform();
Vector3f up = new Vector3f();
up.set(0.0f, 1.0f, 0.0f);
//
// Setup geometry
//
float bodySize = 0.25f;
float legLength = 0.45f;
float foreLegLength = 0.75f;
shapes[0] = new CapsuleShape(bodySize, 0.10f);
int i;
for (i = 0; i < NUM_LEGS; i++) {
shapes[1 + 2 * i] = new CapsuleShape(0.10f, legLength);
shapes[2 + 2 * i] = new CapsuleShape(0.08f, foreLegLength);
}
//
// Setup rigid bodies
//
float height = 0.5f;
Transform offset = new Transform();
offset.setIdentity();
offset.origin.set(positionOffset);
// root
Vector3f root = new Vector3f();
root.set(0.0f, height, 0.0f);
Transform transform = new Transform();
transform.setIdentity();
transform.origin.set(root);
tmpTrans.mul(offset, transform);
if (fixed) {
bodies[0] = localCreateRigidBody(0.0f, tmpTrans, shapes[0]);
} else {
bodies[0] = localCreateRigidBody(1.0f, tmpTrans, shapes[0]);
}
// legs
for (i = 0; i < NUM_LEGS; i++) {
float angle = BulletGlobals.SIMD_2_PI * i / NUM_LEGS;
float sin = (float) Math.sin(angle);
float cos = (float) Math.cos(angle);
transform.setIdentity();
Vector3f boneOrigin = new Vector3f();
boneOrigin.set(
cos * (bodySize + 0.5f * legLength), height, sin * (bodySize + 0.5f * legLength));
transform.origin.set(boneOrigin);
// thigh
Vector3f toBone = new Vector3f(boneOrigin);
toBone.sub(root);
toBone.normalize();
Vector3f axis = new Vector3f();
axis.cross(toBone, up);
Quat4f q = new Quat4f();
QuaternionUtil.setRotation(q, axis, BulletGlobals.SIMD_HALF_PI);
transform.setRotation(q);
tmpTrans.mul(offset, transform);
bodies[1 + 2 * i] = localCreateRigidBody(1.0f, tmpTrans, shapes[1 + 2 * i]);
// shin
transform.setIdentity();
transform.origin.set(
cos * (bodySize + legLength),
height - 0.5f * foreLegLength,
sin * (bodySize + legLength));
tmpTrans.mul(offset, transform);
bodies[2 + 2 * i] = localCreateRigidBody(1.0f, tmpTrans, shapes[2 + 2 * i]);
}
// Setup some damping on the bodies
for (i = 0; i < BODYPART_COUNT; ++i) {
bodies[i].setDamping(0.05f, 0.85f);
bodies[i].setDeactivationTime(0.8f);
bodies[i].setSleepingThresholds(1.6f, 2.5f);
}
//
// Setup the constraints
//
HingeConstraint hingeC;
// ConeTwistConstraint* coneC;
Transform localA = new Transform();
Transform localB = new Transform();
Transform localC = new Transform();
for (i = 0; i < NUM_LEGS; i++) {
float angle = BulletGlobals.SIMD_2_PI * i / NUM_LEGS;
float sin = (float) Math.sin(angle);
float cos = (float) Math.cos(angle);
// hip joints
localA.setIdentity();
localB.setIdentity();
MatrixUtil.setEulerZYX(localA.basis, 0, -angle, 0);
localA.origin.set(cos * bodySize, 0.0f, sin * bodySize);
tmpTrans.inverse(bodies[1 + 2 * i].getWorldTransform(new Transform()));
tmpTrans.mul(tmpTrans, bodies[0].getWorldTransform(new Transform()));
localB.mul(tmpTrans, localA);
hingeC = new HingeConstraint(bodies[0], bodies[1 + 2 * i], localA, localB);
hingeC.setLimit(-0.75f * BulletGlobals.SIMD_2_PI * 0.125f, BulletGlobals.SIMD_2_PI * 0.0625f);
// hingeC.setLimit(-0.1f, 0.1f);
joints[2 * i] = hingeC;
ownerWorld.addConstraint(joints[2 * i], true);
// knee joints
localA.setIdentity();
localB.setIdentity();
localC.setIdentity();
MatrixUtil.setEulerZYX(localA.basis, 0, -angle, 0);
localA.origin.set(cos * (bodySize + legLength), 0.0f, sin * (bodySize + legLength));
tmpTrans.inverse(bodies[1 + 2 * i].getWorldTransform(new Transform()));
tmpTrans.mul(tmpTrans, bodies[0].getWorldTransform(new Transform()));
localB.mul(tmpTrans, localA);
tmpTrans.inverse(bodies[2 + 2 * i].getWorldTransform(new Transform()));
tmpTrans.mul(tmpTrans, bodies[0].getWorldTransform(new Transform()));
localC.mul(tmpTrans, localA);
hingeC = new HingeConstraint(bodies[1 + 2 * i], bodies[2 + 2 * i], localB, localC);
// hingeC.setLimit(-0.01f, 0.01f);
hingeC.setLimit(-BulletGlobals.SIMD_2_PI * 0.0625f, 0.2f);
joints[1 + 2 * i] = hingeC;
ownerWorld.addConstraint(joints[1 + 2 * i], true);
}
}
@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));
}
}
}
public boolean EncloseOrigin() {
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
switch (order) {
// Point
case 0:
break;
// Line
case 1:
{
Vector3f ab = Stack.alloc(Vector3f.class);
ab.sub(simplex[1].w, simplex[0].w);
Vector3f[] b =
new Vector3f[] {
Stack.alloc(Vector3f.class),
Stack.alloc(Vector3f.class),
Stack.alloc(Vector3f.class)
};
b[0].set(1f, 0f, 0f);
b[1].set(0f, 1f, 0f);
b[2].set(0f, 0f, 1f);
b[0].cross(ab, b[0]);
b[1].cross(ab, b[1]);
b[2].cross(ab, b[2]);
float m[] =
new float[] {b[0].lengthSquared(), b[1].lengthSquared(), b[2].lengthSquared()};
Quat4f tmpQuat = Stack.alloc(Quat4f.class);
tmp.normalize(ab);
QuaternionUtil.setRotation(tmpQuat, tmp, cst2Pi / 3f);
Matrix3f r = Stack.alloc(Matrix3f.class);
MatrixUtil.setRotation(r, tmpQuat);
Vector3f w = Stack.alloc(Vector3f.class);
w.set(b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]);
tmp.normalize(w);
Support(tmp, simplex[4]);
r.transform(w);
tmp.normalize(w);
Support(tmp, simplex[2]);
r.transform(w);
tmp.normalize(w);
Support(tmp, simplex[3]);
r.transform(w);
order = 4;
return (true);
}
// Triangle
case 2:
{
tmp1.sub(simplex[1].w, simplex[0].w);
tmp2.sub(simplex[2].w, simplex[0].w);
Vector3f n = Stack.alloc(Vector3f.class);
n.cross(tmp1, tmp2);
n.normalize();
Support(n, simplex[3]);
tmp.negate(n);
Support(tmp, simplex[4]);
order = 4;
return (true);
}
// Tetrahedron
case 3:
return (true);
// Hexahedron
case 4:
return (true);
}
return (false);
}
