public static Vector3 ComputeNewTailPosition(Circle3 intersection, Vector3 tailPosition)
{
// http://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point
// Project child's position onto the plane
var newTailPosition = tailPosition
- Vector3.Dot(intersection.upVector, tailPosition - intersection.origin) * intersection.upVector;
var v = newTailPosition - intersection.origin;
var newPosition = intersection.origin + intersection.radius * v.normalized;
return(newPosition);
}
public static SpringBone.CollisionStatus CheckForCollisionAndReact
(
Vector3 localHeadPosition,
ref Vector3 localTailPosition,
float localTailRadius,
Vector3 sphereLocalOrigin,
float sphereRadius
)
{
var combinedRadius = sphereRadius + localTailRadius;
if ((localTailPosition - sphereLocalOrigin).sqrMagnitude >= combinedRadius * combinedRadius)
{
// Not colliding
return(SpringBone.CollisionStatus.NoCollision);
}
var originToHead = localHeadPosition - sphereLocalOrigin;
if (originToHead.sqrMagnitude <= sphereRadius * sphereRadius)
{
// The head is inside the sphere, so just try to push the tail out
localTailPosition =
sphereLocalOrigin + (localTailPosition - sphereLocalOrigin).normalized * combinedRadius;
return(SpringBone.CollisionStatus.HeadIsEmbedded);
}
var localHeadRadius = (localTailPosition - localHeadPosition).magnitude;
var intersection = new Circle3();
if (ComputeIntersection(
localHeadPosition,
localHeadRadius,
sphereLocalOrigin,
combinedRadius,
ref intersection))
{
localTailPosition = ComputeNewTailPosition(intersection, localTailPosition);
}
return(SpringBone.CollisionStatus.TailCollision);
}
// private
// http://mathworld.wolfram.com/Sphere-SphereIntersection.html
public static bool ComputeIntersection
(
Vector3 originA,
float radiusA,
Vector3 originB,
float radiusB,
ref Circle3 intersection
)
{
var aToB = originB - originA;
var dSqr = aToB.sqrMagnitude;
var d = Mathf.Sqrt(dSqr);
if (d <= 0f)
{
return(false);
}
var radiusASqr = radiusA * radiusA;
var radiusBSqr = radiusB * radiusB;
// Assume a is at the origin and b is at (d, 0 0)
var denominator = 0.5f / d;
var subTerm = dSqr - radiusBSqr + radiusASqr;
var x = subTerm * denominator;
var squaredTerm = subTerm * subTerm;
var intersectionRadius = Mathf.Sqrt(4f * dSqr * radiusASqr - squaredTerm) * denominator;
var upVector = aToB / d;
var origin = originA + x * upVector;
intersection.origin = origin;
intersection.upVector = upVector;
intersection.radius = intersectionRadius;
return(true);
}
public SpringBone.CollisionStatus CheckForCollisionAndReact
(
Vector3 moverHeadPosition,
ref Vector3 moverPosition,
float moverRadius,
ref Vector3 hitNormal
)
{
const float RadiusThreshold = 0.0001f;
if ((linkedRenderer != null && !linkedRenderer.enabled) ||
radius <= RadiusThreshold)
{
return(SpringBone.CollisionStatus.NoCollision);
}
var worldToLocal = transform.worldToLocalMatrix;
var radiusScale = worldToLocal.MultiplyVector(new Vector3(1f, 0f, 0f)).magnitude;
// Lower than start cap
var localHeadPosition = worldToLocal.MultiplyPoint3x4(moverHeadPosition);
var localMoverPosition = worldToLocal.MultiplyPoint3x4(moverPosition);
var localMoverRadius = moverRadius * radiusScale;
var moverIsAboveTop = localMoverPosition.y >= height;
var useSphereCheck = (localMoverPosition.y <= 0f) | moverIsAboveTop;
if (useSphereCheck)
{
var sphereOrigin = new Vector3(0f, moverIsAboveTop ? height : 0f, 0f);
var combinedRadius = localMoverRadius + radius;
if ((localMoverPosition - sphereOrigin).sqrMagnitude >= combinedRadius * combinedRadius)
{
// Not colliding
return(SpringBone.CollisionStatus.NoCollision);
}
var originToHead = localHeadPosition - sphereOrigin;
var isHeadEmbedded = originToHead.sqrMagnitude <= radius * radius;
#if UNITY_EDITOR
RecordSphereCollision(
sphereOrigin,
localMoverPosition,
moverRadius,
isHeadEmbedded ?
SpringBone.CollisionStatus.HeadIsEmbedded :
SpringBone.CollisionStatus.TailCollision);
#endif
if (isHeadEmbedded)
{
// The head is inside the sphere, so just try to push the tail out
var localHitNormal = (localMoverPosition - sphereOrigin).normalized;
localMoverPosition = sphereOrigin + localHitNormal * combinedRadius;
moverPosition = transform.TransformPoint(localMoverPosition);
hitNormal = transform.TransformDirection(localHitNormal).normalized;
return(SpringBone.CollisionStatus.HeadIsEmbedded);
}
var localHeadRadius = (localMoverPosition - localHeadPosition).magnitude;
var intersection = new Circle3();
if (SpringSphereCollider.ComputeIntersection(
localHeadPosition,
localHeadRadius,
sphereOrigin,
combinedRadius,
ref intersection))
{
localMoverPosition = SpringSphereCollider.ComputeNewTailPosition(intersection, localMoverPosition);
moverPosition = transform.TransformPoint(localMoverPosition);
var localHitNormal = (localMoverPosition - sphereOrigin).normalized;
hitNormal = transform.TransformDirection(localHitNormal).normalized;
}
return(SpringBone.CollisionStatus.TailCollision);
}
// Cylinder
var collisionStatus = CheckForCylinderCollisionAndReact(
localHeadPosition, ref moverPosition, localMoverRadius, localMoverPosition, ref hitNormal);
return(collisionStatus);
}