protected float[] m_springDamping = new float[6]; // between 0 and 1 (1 == no damping)
public Generic6DofSpringConstraint(RigidBody rbA, RigidBody rbB, btTransform frameInA, btTransform frameInB, bool useLinearReferenceFrameA)
: base(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
{
for (int i = 0; i < 6; i++)
{
m_springEnabled[i] = false;
m_equilibriumPoint[i] = 0f;
m_springStiffness[i] = 0f;
m_springDamping[i] = 1f;
}
}
//!@}
public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, btTransform frameInA, btTransform frameInB, bool useLinearReferenceFrameA)
: base(TypedConstraintType.D6_CONSTRAINT_TYPE, rbA, rbB)
{
m_linearLimits = new TranslationalLimitMotor();
m_angularLimits = new RotationalLimitMotor[3];
for (int i = 0; i < m_angularLimits.Length; i++)
m_angularLimits[i] = new RotationalLimitMotor();
m_frameInA = frameInA;
m_frameInB = frameInB;
m_useLinearReferenceFrameA = useLinearReferenceFrameA;
m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
m_flags = bt6DofFlags.BT_6DOF_FLAGS_NONE;
m_useSolveConstraintObsolete = D6_USE_OBSOLETE_METHOD;
calculateTransforms();
}
public Generic6DofConstraint(RigidBody rbB, btTransform frameInB, bool useLinearReferenceFrameB)
: base(TypedConstraintType.D6_CONSTRAINT_TYPE, getFixedBody(), rbB)
{
m_linearLimits = new TranslationalLimitMotor();
m_angularLimits = new RotationalLimitMotor[3];
for (int i = 0; i < m_angularLimits.Length; i++)
m_angularLimits[i] = new RotationalLimitMotor();
m_frameInB = frameInB;
m_useLinearReferenceFrameA = useLinearReferenceFrameB;
m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
m_flags = bt6DofFlags.BT_6DOF_FLAGS_NONE;
m_useSolveConstraintObsolete = false;
///not providing rigidbody A means implicitly using worldspace for body A
m_frameInA = rbB.CenterOfMassTransform * m_frameInB;
calculateTransforms();
}
private RigidBody CreateRigidBody(MMDRigid rigid, MMDModel Model,out short group, out MMDMotionState motionStateStart)
{
CollisionShape collision;
RigidBody body;
//衝突スキンの作成
switch (rigid.ShapeType)
{
case 0:
collision = new SphereShape(rigid.ShapeWidth);
break;
case 1:
collision = new BoxShape(new btVector3(rigid.ShapeWidth, rigid.ShapeHeight, rigid.ShapeDepth));
break;
case 2:
collision = new CapsuleShape(rigid.ShapeWidth, rigid.ShapeHeight);
break;
default:
throw new NotImplementedException("不明な剛体タイプ");
}
motionStateStart = new MMDMotionState(rigid, Model);
btVector3 localInertia = btVector3.Zero;
//イナーシャの計算
if (rigid.Type != 0)
collision.calculateLocalInertia(rigid.Weight, out localInertia);
//剛体を作成
body = new RigidBody((rigid.Type != 0 ? rigid.Weight : 0), motionStateStart, collision, localInertia);
//ダンピング値、摩擦、Restitutionをセット
body.setDamping(rigid.LinerDamping, rigid.AngularDamping);
body.Friction = rigid.Friction;
body.Restitution = rigid.Restitution;
//ボーン追従型はKinematicにする
if (rigid.Type == 0)
{
body.ActivationState |= ActivationStateFlags.DISABLE_DEACTIVATION;
body.CollisionFlags = CollisionFlags.CF_KINEMATIC_OBJECT;
if (!string.IsNullOrEmpty(rigid.RelatedBoneName))
Model.BoneManager[rigid.RelatedBoneName].IsPhysics = false;
}
else
{//物理演算型はボーンのフラグをオンにする
if (!string.IsNullOrEmpty(rigid.RelatedBoneName))
Model.BoneManager[rigid.RelatedBoneName].IsPhysics = true;
}
//グループのフラグをセット
group = (short)Math.Pow(2, rigid.GroupIndex);
return body;
}
private Generic6DofSpringConstraint CreateJoint(RigidBody body0, RigidBody body1, MMDJoint joint, MMDModel model)
{
Matrix frameInA, frameInB;
btTransform btFrameInA, btFrameInB;
Matrix jointPos =MMDXMath.CreateMatrixFromYawPitchRoll(joint.Rotation[1], joint.Rotation[0], joint.Rotation[2])
* MMDXMath.CreateTranslationMatrix(joint.Position[0], joint.Position[1], joint.Position[2]);
if (body0.MotionState != null)
{
MMDMotionState motionState = (MMDMotionState)body0.MotionState;
frameInA = MMDXMath.ToMatrix(motionState.GraphicsWorldTrans);
}
else
throw new NotImplementedException("来るハズないのだが");
frameInA = jointPos * model.Transform * Matrix.Invert(frameInA);
if (body1.MotionState != null)
{
MMDMotionState motionState = (MMDMotionState)body1.MotionState;
frameInB = MMDXMath.ToMatrix(motionState.GraphicsWorldTrans);
}
else
throw new NotImplementedException("来るハズないのだが");
frameInB = jointPos * model.Transform * Matrix.Invert(frameInB);
//frameInB = jointPos * Matrix.Invert(MMDMath.ConvertToMatrix(body1.GetWorldTransformSmart()));
MMDXMath.TobtTransform(ref frameInA, out btFrameInA);
MMDXMath.TobtTransform(ref frameInB, out btFrameInB);
Generic6DofSpringConstraint mConstPoint = new Generic6DofSpringConstraint(body0, body1, btFrameInA, btFrameInB, true);
//G6Dof設定用変数の準備
mConstPoint.setLinearLowerLimit(new btVector3(joint.ConstrainPosition1));
mConstPoint.setLinearUpperLimit(new btVector3(joint.ConstrainPosition2));
mConstPoint.setAngularLowerLimit(new btVector3(joint.ConstrainRotation1));
mConstPoint.setAngularUpperLimit(new btVector3(joint.ConstrainRotation2));
System.Diagnostics.Debug.WriteLine(joint.Name);
for(int i=0;i<3;i++)
System.Diagnostics.Debug.WriteLine(joint.ConstrainPosition1[i]);
for (int i = 0; i < 3; i++)
System.Diagnostics.Debug.WriteLine(joint.ConstrainPosition2[i]);
for (int i = 0; i < 3; i++)
System.Diagnostics.Debug.WriteLine(joint.ConstrainRotation1[i]);
for (int i = 0; i < 3; i++)
System.Diagnostics.Debug.WriteLine(joint.ConstrainRotation2[i]);
for (int i = 0; i < 3; i++)
{
mConstPoint.setStiffness(i, joint.SpringPosition[i]);
mConstPoint.enableSpring(i, true);
mConstPoint.setStiffness(i + 3, joint.SpringRotation[i]);
mConstPoint.enableSpring(i + 3, true);
}
mConstPoint.calculateTransforms();
mConstPoint.setEquilibriumPoint();
return mConstPoint;
}
public abstract void removeRigidBody(RigidBody body);
public abstract void addRigidBody(RigidBody body);
