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