Vector3 DebugSphereCollision()
{
if (Vector3.Dot(p2.position - p4.position, normal.normalized) >= collisionRadius)
{
return(Vector3.zero);
}
Line clothLine = new Line()
{
startPos = p1.position, endPos = p2.position
};
Line sphereLine = new Line()
{
startPos = p3.position, endPos = p4.position
};
MathHelper.CollisionResult closestLine = MathHelper.DistBetweenSegments(sphereLine, clothLine);
if (null == closestLine.coords)
{
return(Vector3.zero);
}
Debug.DrawLine(closestLine.coords[1], closestLine.coords[0], Color.white);
Vector3 diff = closestLine.coords[1] - closestLine.coords[0];
Vector3 closestPoint = p4.position + diff;
float dist = diff.magnitude;
if (dist < collisionRadius + EPSILON)
{
Vector3 disp = normal.normalized * (collisionRadius - Vector3.Dot(diff, normal.normalized));
Vector3 posAfterDisp = closestPoint + disp;
Vector3 resultDisp = posAfterDisp - p2.position;
return(resultDisp);
}
return(Vector3.zero);
}
Vector3 HandleSphereCollision(int index, VertWithNorm sphere)
{
if (Vector3.Dot(positions[index] - sphere.pos, sphere.norm.normalized) >= collisionHandler.collisionRadius)
{
return(Vector3.zero);
}
Line clothLine = new Line()
{
startPos = oldPositions[index], endPos = positions[index]
};
Line sphereLine = new Line()
{
startPos = sphere.prevPos, endPos = sphere.pos
};
MathHelper.CollisionResult closestLine = MathHelper.DistBetweenSegments(sphereLine, clothLine);
if (null == closestLine.coords)
{
return(Vector3.zero);
}
Vector3 diff = closestLine.coords[1] - closestLine.coords[0];
Vector3 closestPoint = sphere.pos + diff;
float dist = diff.magnitude;
if (dist < collisionHandler.collisionRadius + EPSILON)
{
Vector3 disp = sphere.norm * (collisionHandler.collisionRadius - Vector3.Dot(diff, sphere.norm));
Vector3 posAfterDisp = closestPoint + disp;
Vector3 resultDisp = posAfterDisp - positions[index];
return(resultDisp);
}
return(Vector3.zero);
}
