public void Fix()
{
if (!_strict)
{
return;
}
var parentLocalMatrix = new Matrix4x4();
_target.LocalMatrix = _target.SkinningMatrix * _target.GlobalOffsetMatrixInv;
if (_target.HasParent)
{
parentLocalMatrix = BulletPyhsicsReactor.GetPoserBoneImage(_poser, _target.Parent).LocalMatrix;
_target.LocalMatrix = parentLocalMatrix.inverse * _target.LocalMatrix;
}
MathUtil.SetTransToMatrix4X4(_target.TotalTranslation + _target.LocalOffset, ref _target.LocalMatrix);
if (_target.HasParent)
{
_target.LocalMatrix = parentLocalMatrix * _target.LocalMatrix;
}
_target.SkinningMatrix = _target.LocalMatrix * _target.GlobalOffsetMatrix;
}
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);
}
}