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 static bool FindTangentPoint
(
Transform transform,
Vector3 localSphereOrigin,
float localSphereRadius,
Vector3 worldFixedPoint,
Vector3 worldMovingPoint,
float worldSegmentRadius,
ref Vector3 tangentPoint
)
{
var fixedPoint = transform.InverseTransformPoint(worldFixedPoint) - localSphereOrigin;
var movingPoint = transform.InverseTransformPoint(worldMovingPoint) - localSphereOrigin;
var otherRadius = transform.InverseTransformDirection(worldSegmentRadius, 0f, 0f).magnitude;
var combinedRadius = localSphereRadius + otherRadius;
var ta = new Vector2(0f, 0f);
var tb = new Vector2(0f, 0f);
var dd = fixedPoint.magnitude;
if (!Circle3.FindCircleTangentPoints(dd, combinedRadius, ref ta, ref tb))
{
// The fixed point is inside the sphere!
return(false);
}
// todo: It seems like we should be able to rotate based on the angle in 3D directly somehow?
var xVector = fixedPoint / dd;
var fixedToMoving = movingPoint - fixedPoint;
var yVector = (fixedToMoving - Vector3.Project(fixedToMoving, xVector)).normalized;
var tangentPoint1 = ta.x * xVector + ta.y * yVector;
var tangentPoint2 = tb.x * xVector + tb.y * yVector;
var isTangentPoint1CloserToMover = (tangentPoint1 - movingPoint).sqrMagnitude < (tangentPoint2 - movingPoint).sqrMagnitude;
var localTangentPoint = isTangentPoint1CloserToMover ? tangentPoint1 : tangentPoint2;
tangentPoint = transform.TransformPoint(localTangentPoint + localSphereOrigin);
return(true);
}
// 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
)
{
if ((linkedRenderer != null &&
!linkedRenderer.enabled) ||
radius <= 0.0001f)
{
return(SpringBone.CollisionStatus.NoCollision);
}
if (needToCacheTransform)
{
CacheTransform();
}
// 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, 0f, 0f);
sphereOrigin.y = moverIsAboveTop ? height : 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
localMoverPosition =
sphereOrigin + (localMoverPosition - sphereOrigin).normalized * combinedRadius;
moverPosition = transform.TransformPoint(localMoverPosition);
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);
}
return(SpringBone.CollisionStatus.TailCollision);
}
// Cylinder
var collisionStatus = CheckForCylinderCollisionAndReact(
localHeadPosition, ref moverPosition, localMoverRadius, localMoverPosition);
return(collisionStatus);
}