public void AddRigidBody(Physics3DRigidBody rb, Components.RigidBodyDesc desc)
{
MotionState motionState;
Matrix4x4 mat = MatrixExt.Transform(desc.Position, desc.Rotation);
rb.defaultPosition = desc.Position;
rb.defaultRotation = desc.Rotation;
motionState = new DefaultMotionState(GetMatrix(mat));
CollisionShape collisionShape;
switch (desc.Shape)
{
case Components.RigidBodyShape.Sphere:
collisionShape = new SphereShape(desc.Dimemsions.X);
break;
case Components.RigidBodyShape.Capsule:
collisionShape = new CapsuleShape(desc.Dimemsions.X, desc.Dimemsions.Y);
break;
case Components.RigidBodyShape.Box:
default:
collisionShape = new BoxShape(GetVector3(desc.Dimemsions));
break;
}
float mass = desc.Mass;
BulletSharp.Math.Vector3 localInertia = new BulletSharp.Math.Vector3();
if (desc.Type == 0)
{
mass = 0;
}
else
{
collisionShape.CalculateLocalInertia(mass, out localInertia);
}
var rigidbodyInfo = new RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia);
rigidbodyInfo.Friction = desc.Friction;
rigidbodyInfo.LinearDamping = desc.LinearDamping;
rigidbodyInfo.AngularDamping = desc.AngularDamping;
rigidbodyInfo.Restitution = desc.Restitution;
rb.rigidBody = new RigidBody(rigidbodyInfo);
rb.rigidBody.ActivationState = ActivationState.DisableDeactivation;
rb.rigidBody.SetSleepingThresholds(0, 0);
if (desc.Type == Components.RigidBodyType.Kinematic)
{
rb.rigidBody.CollisionFlags |= CollisionFlags.KinematicObject;
}
world.AddRigidBody(rb.rigidBody, 1 << desc.CollisionGroup, desc.CollisionMask);
}
public void ResetRigidBody(Physics3DRigidBody rb, Matrix4x4 mat)
{
var worldTransform = GetMatrix(mat);
var rigidBody = rb.rigidBody;
rigidBody.MotionState.SetWorldTransform(ref worldTransform);
rigidBody.CenterOfMassTransform = worldTransform;
rigidBody.InterpolationWorldTransform = worldTransform;
rigidBody.InterpolationWorldTransform = worldTransform;
rigidBody.AngularVelocity = new BulletSharp.Math.Vector3();
rigidBody.LinearVelocity = new BulletSharp.Math.Vector3();
rigidBody.InterpolationAngularVelocity = new BulletSharp.Math.Vector3();
rigidBody.InterpolationLinearVelocity = new BulletSharp.Math.Vector3();
rigidBody.ClearForces();
}
public void AddJoint(Physics3DJoint joint, Physics3DRigidBody r1, Physics3DRigidBody r2, Components.JointDesc desc)
{
var t0 = MatrixExt.Transform(desc.Position, ToQuaternion(desc.Rotation));
Matrix4x4.Invert(t0, out var res);
Matrix4x4.Invert(MatrixExt.Transform(r1.defaultPosition, r1.defaultRotation), out var t1);
Matrix4x4.Invert(MatrixExt.Transform(r2.defaultPosition, r2.defaultRotation), out var t2);
t1 = t0 * t1;
t2 = t0 * t2;
var j = new Generic6DofSpringConstraint(r1.rigidBody, r2.rigidBody, GetMatrix(t1), GetMatrix(t2), true);
joint.constraint = j;
j.LinearLowerLimit = GetVector3(desc.PositionMinimum);
j.LinearUpperLimit = GetVector3(desc.PositionMaximum);
j.AngularLowerLimit = GetVector3(desc.RotationMinimum);
j.AngularUpperLimit = GetVector3(desc.RotationMaximum);
S(0, desc.PositionSpring.X);
S(1, desc.PositionSpring.Y);
S(2, desc.PositionSpring.Z);
S(3, desc.RotationSpring.X);
S(4, desc.RotationSpring.Y);
S(5, desc.RotationSpring.Z);
void S(int index, float f)
{
if (f != 0.0f)
{
j.EnableSpring(index, true);
j.SetStiffness(index, f);
}
else
{
j.EnableSpring(index, false);
}
}
world.AddConstraint(joint.constraint);
}
public void RemoveRigidBody(Physics3DRigidBody rb)
{
world.RemoveRigidBody(rb.rigidBody);
rb.rigidBody.Dispose();
}
public void MoveRigidBody(Physics3DRigidBody rb, Matrix4x4 mat)
{
rb.rigidBody.MotionState.WorldTransform = GetMatrix(mat);
}
