void Update()
{
for (int it = 0; it < 1; it++)
{
//Move toward main target
for (int j = 0; j < joints.Length; j++)
{
joints[j].Evaluate(Tooltip, Target, false);// j < 2);
}
//Find the closest point
for (int j = joints.Length - 1; j >= 0; j--)
{
Vector3 jointPoint = Constraints.ClosestPointOnCapsule(environment.transform.position, joints[j].GetComponent <Collider>() as CapsuleCollider);
Vector3 offset = jointPoint - environment.transform.position;
if (offset.magnitude < environment.radius * environment.transform.lossyScale.x)
{
nearestPoint.parent = joints[j].transform;
nearestPoint.position = jointPoint;
displacementTarget.position = nearestPoint.transform.position + (offset.normalized * ((environment.radius * environment.transform.lossyScale.x) - offset.magnitude));
Debug.DrawLine(nearestPoint.position, displacementTarget.position);
//Move away from environment
for (int k = j; k < joints.Length; k++)
{
joints[k].Evaluate(nearestPoint, displacementTarget);
}
}
}
}
}
//Returns the signed distance between the nearest joint and the nearest obstructor for this configuration (sphere and ground plane in this instance)
float distanceToNearestObstructor()
{
float distance = 100f;
for (int j = joints.Length - 1; j >= 0; j--)
{
float jointDistance = 100f;
//Closest point on Sphere
Vector3 jointPoint = Constraints.ClosestPointOnCapsule(sphere.transform.position, joints[j].GetComponent <Collider>() as CapsuleCollider);
Vector3 offset = jointPoint - sphere.transform.position;
float tempDist = offset.magnitude - (sphere.radius * sphere.transform.lossyScale.x);
jointDistance = (jointDistance > tempDist) ? tempDist : jointDistance;
//Closest point on Plane
tempDist = distanceCapsuleToPlane(joints[j].GetComponent <Collider>() as CapsuleCollider, floor.position, -floor.forward.normalized);
jointDistance = (jointDistance > tempDist) ? tempDist : jointDistance;
//Color the colliding joint appropriately
joints[j].transform.GetChild(1).GetComponent <Renderer>().material.color = Color.Lerp(Color.red, Color.green, jointDistance * 30f);
//Update the minimum distance
distance = (distance > jointDistance) ? jointDistance : distance;
}
return(distance - inflation);
}