/// <summary>
/// Se crea una capsula a partir de un radio, altura, posicion, masa y si se dedea o no calcular
/// la inercia. Esto es importante ya que sin inercia no se generan rotaciones que no se
/// controlen en forma particular.
/// </summary>
/// <param name="radius">Radio de la Capsula</param>
/// <param name="height">Altura de la Capsula</param>
/// <param name="position">Posicion de la Capsula</param>
/// <param name="mass">Masa de la Capsula</param>
/// <param name="needInertia">Booleano para el momento de inercia de la Capsula</param>
/// <returns>Rigid Body de una Capsula</returns>
public static RigidBody CreateCapsule(float radius, float height, TGCVector3 position, float mass, bool needInertia)
{
//Creamos el shape de la Capsula a partir de un radio y una altura.
var capsuleShape = new CapsuleShape(radius, height);
//Armamos las transformaciones que luego formaran parte del cuerpo rigido de la capsula.
var capsuleTransform = TGCMatrix.Identity;
capsuleTransform.Origin = position;
var capsuleMotionState = new DefaultMotionState(capsuleTransform.ToBsMatrix);
RigidBodyConstructionInfo capsuleRigidBodyInfo;
//Calculamos o no el momento de inercia dependiendo de que comportamiento
//queremos que tenga la capsula.
if (!needInertia)
{
capsuleRigidBodyInfo = new RigidBodyConstructionInfo(mass, capsuleMotionState, capsuleShape);
}
else
{
var capsuleInertia = capsuleShape.CalculateLocalInertia(mass);
capsuleRigidBodyInfo = new RigidBodyConstructionInfo(mass, capsuleMotionState, capsuleShape, capsuleInertia);
}
var localCapsuleRigidBody = new RigidBody(capsuleRigidBodyInfo);
localCapsuleRigidBody.LinearFactor = TGCVector3.One.ToBsVector;
//Dado que hay muchos parametros a configurar el RigidBody lo ideal es que
//cada caso se configure segun lo que se necesite.
return(localCapsuleRigidBody);
}
/// <summary>
/// Se crea una capsula a partir de un radio, una altura y una posicion.
/// Los valores de la masa y el calculo de inercia asociado estan fijos para que no haya comportamiento erratico.
/// </summary>
/// <param name="radius"></param>
/// <param name="height"></param>
/// <param name="position"></param>
/// <returns></returns>
public static RigidBody CreateCapsule(float radius, float height, TGCVector3 position)
{
//Creamos el shape de la Capsula a partir de un radio y una altura.
var caspsuleShape = new CapsuleShape(radius, height);
//Armamos las transformaciones que luego formaran parte del cuerpo rigido de la capsula.
var capsuleTransform = TGCMatrix.Identity;
capsuleTransform.Origin = position;
var capsuleMotionState = new DefaultMotionState(Matrix.Translation(position.ToBsVector));
// Utilizamos una masa muy grande (1000 Kg) para calcular el momento de inercia de forma que la capsula no
// genere una rotacion y termine volcando.
var capsuleInertia = caspsuleShape.CalculateLocalInertia(100000);
// Aqui usamos una masa bastante baja (1 Kg) para que cuando se arme el cuerpo rigido y se intente aplicar
// un impulso se facil de mover la capsula.
var capsuleRigidBodyInfo = new RigidBodyConstructionInfo(1, capsuleMotionState, caspsuleShape, capsuleInertia);
var localCapsuleRigidBody = new RigidBody(capsuleRigidBodyInfo);
localCapsuleRigidBody.LinearFactor = TGCVector3.One.ToBsVector;
localCapsuleRigidBody.SetDamping(0.5f, 0f);
localCapsuleRigidBody.Restitution = 0f;
localCapsuleRigidBody.Friction = 1;
return(localCapsuleRigidBody);
}
protected override void RebuildRigidBody()
{
lock (PhysicsSimulation.Locker)
{
World?.Physics.World.RemoveRigidBody(RigidBody);
_capsuleShape?.Dispose();
RigidBody?.Dispose();
_capsuleShape = new CapsuleShape(_capsuleRadius, _capsuleHeight / 2);
var mass = GetMass();
var intertia = _capsuleShape.CalculateLocalInertia(mass);
var constructionInfo = new RigidBodyConstructionInfo(mass, _motionState, _capsuleShape, intertia);
RigidBody = new RigidBody(constructionInfo)
{
AngularFactor = new BulletSharp.Math.Vector3(0, 0, 0),
CollisionFlags = CollisionFlags.CharacterObject,
ActivationState = ActivationState.DisableDeactivation,
UserObject = this
};
RigidBody.UpdateInertiaTensor();
World?.Physics.World.AddRigidBody(RigidBody);
}
}
private static RigidBody CreateRigidBody(TGCVector3 position, float mass, CapsuleShape capsule)
{
var inertia = capsule.CalculateLocalInertia(mass);
var transform = TGCMatrix.Translation(position);
var motionState = new DefaultMotionState(transform.ToBsMatrix);
var rigidBodyInfo = new RigidBodyConstructionInfo(mass, motionState, capsule, inertia);
return(new RigidBody(rigidBodyInfo)
{
AngularFactor = Vector3.UnitY
});
}
public override void AddPoser(Poser poser)
{
var model = poser.Model;
if (_rigidBodies.ContainsKey(poser))
{
return;
}
poser.ResetPosing();
var motionStates = new List <PoserMotionState>();
_motionStates.Add(poser, motionStates);
var rigidBodies = new List <RigidBody>();
_rigidBodies.Add(poser, rigidBodies);
var constraints = new List <Generic6DofSpringConstraint>();
_constraints.Add(poser, constraints);
foreach (var body in model.Rigidbodies)
{
var bodyDimension = body.Dimemsions;
CollisionShape btShape = null;
var btMass = 0.0f;
var btLocalInertia = new Vector3(0.0f, 0.0f, 0.0f);
switch (body.Shape)
{
case MmdRigidBody.RigidBodyShape.RigidShapeSphere:
btShape = new SphereShape(bodyDimension.x);
break;
case MmdRigidBody.RigidBodyShape.RigidShapeBox:
btShape = new BoxShape(new Vector3(bodyDimension.x, bodyDimension.y,
bodyDimension.z));
break;
case MmdRigidBody.RigidBodyShape.RigidShapeCapsule:
btShape = new CapsuleShape(bodyDimension.x, bodyDimension.y);
break;
default:
throw new ArgumentOutOfRangeException();
}
if (body.Type != MmdRigidBody.RigidBodyType.RigidTypeKinematic)
{
btMass = body.Mass;
btShape.CalculateLocalInertia(btMass, out btLocalInertia);
}
var bodyTransform = MathUtil.QuaternionToMatrix4X4(MathUtil.YxzToQuaternion(body.Rotation));
MathUtil.SetTransToMatrix4X4(body.Position, ref bodyTransform);
var btBodyTransform = new Matrix();
MathUtil.UnityMatrixToBulletMatrix(bodyTransform, ref btBodyTransform);
var btMotionState = new PoserMotionState(poser, body, btBodyTransform);
var btInfo =
new RigidBodyConstructionInfo(btMass, btMotionState, btShape, btLocalInertia)
{
LinearDamping = body.TranslateDamp,
AngularDamping = body.RotateDamp,
Restitution = body.Restitution,
Friction = body.Friction
};
var btRigidBody = new RigidBody(btInfo)
{
ActivationState = ActivationState.DisableDeactivation
};
if (body.Type == MmdRigidBody.RigidBodyType.RigidTypeKinematic)
{
btRigidBody.CollisionFlags = btRigidBody.CollisionFlags | CollisionFlags.KinematicObject;
}
_world.AddRigidBody(btRigidBody, (short)(1 << body.CollisionGroup), (short)body.CollisionMask);
#if MMD_PHYSICS_DEBUG
CreateUnityCollisionObjectProxy(btRigidBody, body.Name);
#endif
motionStates.Add(btMotionState);
rigidBodies.Add(btRigidBody);
}
foreach (var constraint in model.Constraints)
{
var btBody1 = rigidBodies[constraint.AssociatedRigidBodyIndex[0]];
var btBody2 = rigidBodies[constraint.AssociatedRigidBodyIndex[1]];
var positionLowLimit = constraint.PositionLowLimit;
var positionHiLimit = constraint.PositionHiLimit;
var rotationLoLimit = constraint.RotationLowLimit;
var rotationHiLimit = constraint.RotationHiLimit;
var constraintTransform = MathUtil.QuaternionToMatrix4X4(MathUtil.YxzToQuaternion(constraint.Rotation));
MathUtil.SetTransToMatrix4X4(constraint.Position, ref constraintTransform);
var btConstraintTransform = new Matrix();
MathUtil.UnityMatrixToBulletMatrix(constraintTransform, ref btConstraintTransform);
var btLocalizationTransform1 = btConstraintTransform * Matrix.Invert(btBody1.WorldTransform); //TODO 验证这个和mmdlib里算出来的是否一样
var btLocalizationTransform2 = btConstraintTransform * Matrix.Invert(btBody2.WorldTransform);
var btConstraint = new Generic6DofSpringConstraint(btBody1, btBody2, btLocalizationTransform1,
btLocalizationTransform2, true)
{
LinearLowerLimit =
new Vector3(positionLowLimit.x, positionLowLimit.y, positionLowLimit.z),
LinearUpperLimit =
new Vector3(positionHiLimit.x, positionHiLimit.y, positionHiLimit.z),
AngularLowerLimit =
new Vector3(rotationLoLimit.x, rotationLoLimit.y, rotationLoLimit.z),
AngularUpperLimit =
new Vector3(rotationHiLimit.x, rotationHiLimit.y, rotationHiLimit.z)
};
for (var j = 0; j < 3; ++j)
{
btConstraint.SetStiffness(j, constraint.SpringTranslate[j]);
btConstraint.EnableSpring(j, true);
btConstraint.SetStiffness(j + 3, constraint.SpringRotate[j]);
btConstraint.EnableSpring(j + 3, true);
}
_world.AddConstraint(btConstraint);
constraints.Add(btConstraint);
}
}
