public void RemovePhysics(Physics3DScene physics3DScene)
{
for (int j = 0; j < physics3DRigidBodys.Count; j++)
{
physics3DScene.RemoveRigidBody(physics3DRigidBodys[j]);
}
for (int j = 0; j < physics3DJoints.Count; j++)
{
physics3DScene.RemoveJoint(physics3DJoints[j]);
}
physics3DRigidBodys.Clear();
physics3DJoints.Clear();
}
public void SetPhysicsPose(Physics3DScene physics3DScene)
{
for (int i = 0; i < rigidBodyDescs.Count; i++)
{
var desc = rigidBodyDescs[i];
if (desc.Type != 0)
{
continue;
}
int index = desc.AssociatedBoneIndex;
Matrix4x4 mat2 = Matrix4x4.CreateFromQuaternion(desc.Rotation) * Matrix4x4.CreateTranslation(desc.Position) * bones[index].GeneratedTransform * LocalToWorld;
physics3DScene.MoveRigidBody(physics3DRigidBodys[i], mat2);
}
}
public void AddPhysics(Physics3DScene physics3DScene)
{
for (int j = 0; j < rigidBodyDescs.Count; j++)
{
physics3DRigidBodys.Add(new Physics3DRigidBody());
var desc = rigidBodyDescs[j];
physics3DScene.AddRigidBody(physics3DRigidBodys[j], desc.Position, desc.Rotation, desc.Dimemsions, desc.Mass, desc.Restitution, desc.Friction, desc.TranslateDamp, desc.RotateDamp, (byte)desc.Shape, (byte)desc.Type, desc.CollisionGroup, desc.CollisionMask);
}
for (int j = 0; j < jointDescs.Count; j++)
{
physics3DJoints.Add(new Physics3DJoint());
var desc = jointDescs[j];
physics3DScene.AddJoint(physics3DJoints[j], desc.Position, MMDBoneComponent.ToQuaternion(desc.Rotation), physics3DRigidBodys[desc.AssociatedRigidBodyIndex1], physics3DRigidBodys[desc.AssociatedRigidBodyIndex2],
desc.PositionMinimum, desc.PositionMaximum, desc.RotationMinimum, desc.RotationMaximum, desc.PositionSpring, desc.RotationSpring);
}
}
public void TransformToNew(Physics3DScene physics3DScene, Vector3 position, Quaternion rotation)
{
LocalToWorld = Matrix4x4.CreateFromQuaternion(rotation) * Matrix4x4.CreateTranslation(position);
Matrix4x4.Invert(LocalToWorld, out WorldToLocal);
for (int i = 0; i < rigidBodyDescs.Count; i++)
{
var desc = rigidBodyDescs[i];
if (desc.Type != 0)
{
continue;
}
int index = desc.AssociatedBoneIndex;
var bone = bones[index];
Matrix4x4.Decompose(bone.GeneratedTransform, out _, out Quaternion rot, out Vector3 trans);
Vector3 pos = Vector3.Transform(bone.staticPosition, bone.GeneratedTransform * LocalToWorld);
physics3DScene.MoveRigidBody(physics3DRigidBodys[i], pos, rot);
}
}
public void ResetPhysics(Physics3DScene physics3DScene)
{
ComputeMatricesData();
for (int i = 0; i < rigidBodyDescs.Count; i++)
{
var desc = rigidBodyDescs[i];
if (desc.Type == 0)
{
continue;
}
int index = desc.AssociatedBoneIndex;
if (index == -1)
{
continue;
}
var mat1 = bones[index].GeneratedTransform * LocalToWorld;
Matrix4x4.Decompose(mat1, out _, out var rot, out _);
physics3DScene.ResetRigidBody(physics3DRigidBodys[i], Vector3.Transform(desc.Position, mat1), rot * desc.Rotation);
}
}
public void SetPoseAfterPhysics(Physics3DScene physics3DScene)
{
Matrix4x4.Decompose(WorldToLocal, out _, out var q1, out var t1);
for (int i = 0; i < rigidBodyDescs.Count; i++)
{
var desc = rigidBodyDescs[i];
if (desc.Type == 0)
{
continue;
}
int index = desc.AssociatedBoneIndex;
if (index == -1)
{
continue;
}
bones[index]._generatedTransform = Matrix4x4.CreateTranslation(-desc.Position) * Matrix4x4.CreateFromQuaternion(Translate(physics3DScene.GetRigidBodyRotation(physics3DRigidBodys[i]) / desc.Rotation * q1))
* Matrix4x4.CreateTranslation(Vector3.Transform(physics3DScene.GetRigidBodyPosition(physics3DRigidBodys[i]), WorldToLocal));
}
UpdateMatrices(PhysicsNeedUpdateMatIndexs);
UpdateAppendBones();
}
