private static void ResolveCapsuleCollision(
Vector3 worldTail,
Vector3 worldPosition,
BlittableTransform headTransform,
BlittableJoint joint,
BlittableCollider collider,
BlittableLogic logic,
ref Vector3 nextTail)
{
var P = worldTail - worldPosition;
var Q = headTransform.position - worldPosition;
var dot = Vector3.Dot(P, Q);
if (dot <= 0)
{
// head側半球の球判定
ResolveSphereCollision(joint, collider, worldPosition, headTransform, logic, ref nextTail);
}
var t = dot / P.magnitude;
if (t >= 1.0f)
{
// tail側半球の球判定
ResolveSphereCollision(joint, collider, worldTail, headTransform, logic, ref nextTail);
}
// head-tail上の m_transform.position との最近点
var p = worldPosition + P * t;
ResolveSphereCollision(joint, collider, p, headTransform, logic, ref nextTail);
}
private static void ResolveSphereCollision(
BlittableJoint joint,
BlittableCollider collider,
Vector3 worldPosition,
BlittableTransform headTransform,
BlittableLogic logic,
ref Vector3 nextTail)
{
var r = joint.radius + collider.radius;
if (Vector3.SqrMagnitude(nextTail - worldPosition) <= (r * r))
{
// ヒット。Colliderの半径方向に押し出す
var normal = (nextTail - worldPosition).normalized;
var posFromCollider = worldPosition + normal * (joint.radius + collider.radius);
// 長さをboneLengthに強制
nextTail = headTransform.position + (posFromCollider - headTransform.position).normalized * logic.length;
}
}
