public void RotateWorldAxis(float angle, Vector3 axis) { Matrix world_coordinate = Matrix.Identity; if (parent != null) { world_coordinate = parent.GetWorldCoordinate(); } foreach (TMOMat i in frame_matrices) { i.RotateWorldAxis(angle, axis, world_coordinate); } }
/// <summary> /// Cyclic-Coordinate-Descent (CCD) 法による逆運動学の実装です。 /// </summary> /// <param name="effector">エフェクタnode</param> /// <param name="node">対象node</param> /// <param name="target">目標</param> public void Solve(TMONode effector, TMONode node, Vector3 target) { Vector3 worldTargetP = target; Vector3 worldEffectorP = effector.GetWorldPosition(); Vector3 worldNodeP = node.GetWorldPosition(); Matrix invCoord = Matrix.Invert(node.GetWorldCoordinate()); Vector3 localEffectorP = Vector3.TransformCoordinate(worldEffectorP, invCoord); Vector3 localTargetP = Vector3.TransformCoordinate(worldTargetP, invCoord); Quaternion q; if (RotationVectorToVector(localEffectorP, localTargetP, out q)) { node.Rotation = q * node.Rotation; } if ((localEffectorP - localTargetP).LengthSq() < 0.1f) { solved = true; } }