public void reset() {
world.removeRigidBody(rigidBody);
trans = new Transform();
trans.setIdentity();
trans.origin.set(originPosition.x, originPosition.y, originPosition.z);
state = new DefaultMotionState(trans);
init(this.restitution, this.friction, this.mass);
}
public void destroy() {
int i;
// Remove all constraints
for (i = 0; i < JOINT_COUNT; ++i) {
ownerWorld.removeConstraint(joints[i]);
// joints[i].destroy();
joints[i] = null;
}
// Remove all bodies and shapes
for (i = 0; i < BodyPart.BODYPART_COUNT.ordinal(); ++i) {
ownerWorld.removeRigidBody(bodies[i]);
// bodies[i].getMotionState().destroy();
bodies[i].destroy();
bodies[i] = null;
// shapes[i].destroy();
shapes[i] = null;
}
}
/**
* @param restitution反弹系数
* @param friction摩擦力系数
* @param mass
*/
public void init(float restitution, float friction, float mass) {
this.restitution = restitution;
this.friction = friction;
this.mass = mass;
intertia = new javax.vecmath.Vector3f();
RigidBodyConstructionInfo info;
if (mass == 0) {
info = new RigidBodyConstructionInfo(this.mass, state, shape);
} else {
shape.calculateLocalInertia(mass, intertia);
info = new RigidBodyConstructionInfo(this.mass, state, shape, intertia);
}
rigidBody = new RigidBody(info);
rigidBody.setFriction(friction);
rigidBody.setRestitution(restitution);
world.addRigidBody(rigidBody);
}
private RigidBody localCreateRigidBody(
float mass, Transform startTransform, CollisionShape shape) {
boolean isDynamic = (mass != 0f);
Vector3f localInertia = new Vector3f();
localInertia.set(0f, 0f, 0f);
if (isDynamic) {
shape.calculateLocalInertia(mass, localInertia);
}
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia);
RigidBody body = new RigidBody(rbInfo);
ownerWorld.addRigidBody(body);
return body;
}
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);
}
}
