/// <summary>
/// クラッチをつなぎ，クラッチが離れている間の姿勢の変化によるフセット値を更新する．
/// 成功した場合はtrueを返し，クラッチが離れていない場合は何もせずにfalseを返す．
/// </summary>
/// <returns>
/// true: 成功
/// false: 失敗
/// </returns>
public bool EngageClutch()
{
if (clutchEngaged)
{
return(false);
}
clutchedPositionOffset = clutchedPosition - rawPose.position;
pose.position = RotationOffset * (rawPose.position + clutchedPositionOffset) + PositionOffset;
pose.rotation = RotationOffset * rawPose.rotation;
pose.velocity = Vector3.zero;
pose.angularVelocity = Vector3.zero;
prevPose = pose;
if (holdingObject != null)
{
Quaternion q = QuaternionUtility.Rotate(model.pointerOrigin.rotation, RotationOffset * clutchedRotation);
model.pointerOrigin.rotation = Quaternion.Inverse(q) * pose.rotation;
}
clutchEngaged = true;
return(true);
}
//
public Vector3 CalcTorque(Pose pointer, Rigidbody rigidbody)
{
Quaternion pointerRotation = QuaternionUtility.Rotate(pointerOrigin.rotation, pointer.rotation);
Quaternion rigidbodyRotation = QuaternionUtility.Rotate(rigidbodyOrigin.rotation, rigidbody.rotation);
Quaternion qd;
if (Quaternion.Dot(pointerRotation, rigidbodyRotation) > 0)
{
qd = QuaternionUtility.Subtract(pointerRotation, rigidbodyRotation);
}
else
{
qd = QuaternionUtility.Add(pointerRotation, rigidbodyRotation);
}
Quaternion conj = QuaternionUtility.Conjugated(pointerRotation);
Quaternion temp = qd * conj;
Vector3 angle = Vector3.zero;
angle.x = temp.x * 2.0f;
angle.y = temp.y * 2.0f;
angle.z = temp.z * 2.0f;
if (angle.x > Mathf.PI)
{
angle.x -= Mathf.PI * 2;
}
else if (angle.x < -Mathf.PI)
{
angle.x += Mathf.PI * 2;
}
if (angle.y > Mathf.PI)
{
angle.y -= Mathf.PI * 2;
}
else if (angle.y < -Mathf.PI)
{
angle.y += Mathf.PI * 2;
}
if (angle.z > Mathf.PI)
{
angle.z -= Mathf.PI * 2;
}
else if (angle.z < -Mathf.PI)
{
angle.z += Mathf.PI * 2;
}
Vector3 angularVelocity = pointer.angularVelocity - rigidbody.angularVelocity;
return(SpringK * angle + DamperB * angularVelocity);
}