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); }