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;
}
}
