private bool UpdateChildBones() { for (int i = 0; i < Childs.Length; i++) { var child = Childs[i]; Matrix4 invMat = ~child.ChildBone.WorldMat; Vector3 ikPos = (Vector4)IKBone.WorldMat.Translation * invMat; Vector3 targetPos = (Vector4)TargetBone.WorldMat.Translation * invMat; if ((ikPos - targetPos).Length() < 0.01F) { return(true); } var scaling = new Matrix3(0, 1, 1); var ikVec = +(ikPos * scaling); var targetVec = +(targetPos * scaling); float angle = (float)Math.Acos(ikVec * targetVec); angle = MathUtil.Clamp(angle, -LimitAngle, LimitAngle); var axis = targetVec ^ ikVec; if (Math.Abs(angle) < 0.0001F || axis.Length() < 0.0001F) { continue; } Quaternion rot = Quaternion.RotationAxis(axis, angle); if (child.Limit) { rot = ClampEular(rot, child.AngleMin, child.AngleMax); } child.ChildBone.Rot = rot * child.ChildBone.Rot; child.ChildBone.UpdateLocalMatrix(); for (int j = i; j >= 0; j--) { Childs[j].ChildBone.UpdateWorldMatrix(); } TargetBone.UpdateWorldMatrix(); } return(false); }
public void Write(string Location) { using (BinaryWriter bw = new BinaryWriter(File.OpenWrite(Location))) { foreach (FXTrack Track in this.Tracks) { Track.Write(bw); } bw.Write((UInt32)1); bw.Write((UInt32)1); bw.Write((UInt32)this.Tracks.Count); foreach (string TargetBone in this.TargetBones) { bw.Write(TargetBone.PadRight(64, '\u0000').ToCharArray()); } } }
public void Update() { IKBone.UpdateWorldMatrix(); for (int j = Childs.Length - 1; j >= 0; j--) { Childs[j].ChildBone.UpdateWorldMatrix(); } TargetBone.UpdateWorldMatrix(); for (int i = 0; i < 255; i++) { if (UpdateChildBones()) { return; } } }