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);
}
}
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);
}
