public void Reader(ContentReader reader)
{
//count of chains
IKChainCount = reader.ReadUInt16();
//for all chain
ikChains = new IkChain[IKChainCount];
for (int i = 0; i < IKChainCount; i++)
{
IkChains[i] = new IkChain();
IkChains[i].Read(reader);
}
}
/// <summary>
/// limit the bone rotate
/// </summary>
/// <param name="bonePos"></param>
/// <returns></returns>
Transform AdjustTotalBoneMove(int boneIndex, IkChain chain, float deltaAngle)
{
bool result = false;
for (int i = 0; i < chain.IkNodeCount; i++)
{
if (tempBoneList[boneIndex].Name == "左ひざ" || tempBoneList[boneIndex].Name == "右ひざ")
{
result = true;
}
}
if (!result)
{
return(boneLocalTransform[boneIndex]);//don't change anything
}
Matrix boneTrans = boneLocalTransform[boneIndex].ToMatrix();
Matrix bonePose = boneLocalPose[boneIndex].ToMatrix();
Matrix moveMat = boneTrans * Matrix.Invert(bonePose);
//get rotation & translation & ^=___=^
Vector3 temp, trans;
Quaternion rot;
moveMat.Decompose(out temp, out rot, out trans);
float YRot, XRot, ZRot;
DecompositeQuaternion(rot, out YRot, out XRot, out ZRot);
//Watch Out!
//limit rotation angle
if (XRot < -MathHelper.PiOver2)
{
XRot = -MathHelper.PiOver2;
}
else if (XRot > -MathHelper.ToRadians(3f))
{
XRot = -MathHelper.ToRadians(3f);
}
//XRot=XRot>0?-XRot:XRot;
//we dont change Yrot and Zrot
//if (bFirstIteration)
//{
//XRot = -Math.Abs(deltaAngle);
//}
//if(YRot<0.9f)
if (!(Math.Abs(YRot) > 1 && Math.Abs(ZRot) > 2))//&& Math.Abs(XRot) < 1
{
YRot = 0.0f;
ZRot = 0.0f;
}
return(new Transform(trans, Quaternion.CreateFromYawPitchRoll(YRot, XRot, ZRot)) * boneLocalPose[boneIndex]);
}
/// <summary>
/// apply transform to world transform
/// </summary>
/// <param name="chain"></param>
void ApplyTransform(IkChain chain)
{
Transform boneBase = GetWorldTransform(BoneParent[chain.IkNodes[0]]);
for (int i = 0; i < chain.IkNodeCount; i++)
{
if (i == 0)
{
boneFinalPos[chain.IkNodes[i]] = boneLocalTransform[chain.IkNodes[i]] * boneBase;
}
else
{
boneFinalPos[chain.IkNodes[i]] = boneLocalTransform[chain.IkNodes[i]] * boneFinalPos[chain.IkNodes[i - 1]];
}
}
}
void Solve(Vector3 targetPos, IkChain chain)
{
ushort maxIterations = chain.MaxIterationCount;
Vector3 localTargetPos = Vector3.One;
Vector3 localEffectorPos = Vector3.Zero;
//[75,8,7,6]---->6
//[6,7,8,75]---->6
Transform IKBase = GetWorldTransform(BoneParent[chain.IkNodes[0]]);
#if true //!WINDOWS_PHONE
for (int it = 0; it < maxIterations; it++)
#else
for (int it = 0; it < 1; it++)
#endif
{
//6,7,8---order:8-7-6
for (int i = chain.IkNodeCount - 2; i >= 0; i--)
{
Transform qTrans = IKBase;
for (int j = 0; j <= i; j++)//6->7->8
{
qTrans = boneLocalTransform[chain.IkNodes[j]] * qTrans;
}
Transform objectLoc = qTrans;
//get new end
for (int j = i + 1; j < chain.IkNodeCount; j++)//6->7->8
{
objectLoc = boneLocalTransform[chain.IkNodes[j]] * objectLoc;
}
Matrix invCoord = Matrix.Invert(qTrans.ToMatrix());
//to node i coordinate
localEffectorPos = Vector3.Transform(objectLoc.Pos, invCoord);
localTargetPos = Vector3.Transform(targetPos, invCoord);
//if we reach to the target
if ((localEffectorPos - localTargetPos).LengthSquared() < 1.0e-8f)//
{
return;
}
Vector3 basis2Effector = Vector3.Normalize(localEffectorPos);
Vector3 basis2Target = Vector3.Normalize(localTargetPos);
//rotate angle
float rotationDotProduct = Vector3.Dot(basis2Effector, basis2Target);
float rotationAngle = (float)Math.Acos(rotationDotProduct);
//limit the angle TODO:Control weight
if (rotationAngle > MathHelper.Pi * chain.MaxAngleBetween)
{
rotationAngle = MathHelper.Pi * chain.MaxAngleBetween;
}
else if (rotationAngle < -MathHelper.Pi * chain.MaxAngleBetween)
{
rotationAngle = -MathHelper.Pi * chain.MaxAngleBetween;
}
if (!float.IsNaN(rotationAngle))
{
//we get a valid angle
Vector3 rotationAxis;
rotationAxis = Vector3.Cross(basis2Effector, basis2Target);
rotationAxis.Normalize();
//fix the bone's rotation
if ((!float.IsNaN(rotationAxis.X)) &&
(!float.IsNaN(rotationAxis.Y)) &&
(!float.IsNaN(rotationAxis.Z)))
{
boneLocalTransform[chain.IkNodes[i]] = new Transform(new Vector3(0, 0, 0), Quaternion.CreateFromAxisAngle(rotationAxis, rotationAngle)) *
boneLocalTransform[chain.IkNodes[i]];
}
boneLocalTransform[chain.IkNodes[i]] = AdjustTotalBoneMove(chain.IkNodes[i], chain, rotationAngle);
}
}
if (bFirstIteration)
{
bFirstIteration = false;
}
}
}
