using Microsoft.Xna.Framework;
using BulletCLI;
using MikuMikuDance.XNA.Model.ModelData;

namespace MikuMikuDance.XNA.Model
{
    /// <summary>
    /// MMDからBulletのConstraintを使いやすくするためのラッパーライブラリ
    /// 現在コメントアウト
    /// </summary>
    class MMDPhysicJoint
    {
        //拘束(MikuMikuDanceはGeneric6DofSpringConstraintを使ってる)
        Generic6DofSpringConstraint mConstPoint;
        
        public MMDPhysicJoint(RigidBody body0, RigidBody body1, MMDJoint joint, DiscreteDynamicsWorld world)
        {
            Matrix frameInA, frameInB;
            Matrix jointPos = Matrix.CreateFromYawPitchRoll(joint.Rotation[1], joint.Rotation[0], joint.Rotation[2])
                * Matrix.CreateTranslation(joint.Position[0], joint.Position[1], joint.Position[2]);
            frameInA = jointPos * Matrix.Invert(body0.GetWorldTransformSmart());
            frameInB = jointPos * Matrix.Invert(body1.GetWorldTransformSmart());
            mConstPoint = new Generic6DofSpringConstraint(body0, body1, frameInA, frameInB);

            //G6Dof設定用変数の準備
            Vector3 minDist = MMDMath.VectorFromArray(joint.ConstrainPosition1);
            Vector3 maxDist = MMDMath.VectorFromArray(joint.ConstrainPosition2);
            Vector3 minRotate = MMDMath.VectorFromArray(joint.ConstrainRotation1);
            Vector3 maxRotate = MMDMath.VectorFromArray(joint.ConstrainRotation2);
            
            mConstPoint.SetLinearLowerLimit(minDist);
            mConstPoint.SetLinearUpperLimit(maxDist);
            mConstPoint.SetAngularLowerLimit(minRotate);
            mConstPoint.SetAngularUpperLimit(maxRotate);
            for (int i = 0; i < 3; i++)
            {
                mConstPoint.SetStiffness(i, joint.SpringPosition[i]);
                mConstPoint.SetStiffness(i + 3, joint.SpringRotation[i]);
            }

            world.AddConstraint(mConstPoint);
        }

    }
}
