114 lines
3.5 KiB
C#
114 lines
3.5 KiB
C#
namespace Fusion.Addons.KCC
|
|
{
|
|
using UnityEngine;
|
|
|
|
public sealed class PenetrationSolver : MonoBehaviour
|
|
{
|
|
public CapsuleCollider Collider;
|
|
public LayerMask CollisionLayerMask = 1;
|
|
public int PenetrationSteps = 8;
|
|
|
|
private Collider[] _hitColliders = new Collider[KCC.CACHE_SIZE];
|
|
private KCCResolver _resolver = new KCCResolver(KCC.CACHE_SIZE);
|
|
|
|
private void OnDrawGizmosSelected()
|
|
{
|
|
if (Collider == null)
|
|
return;
|
|
|
|
Vector3 position = transform.position;
|
|
float radius = Collider.radius;
|
|
float height = Collider.height;
|
|
|
|
position = Depenetrate(position, radius, height, radius);
|
|
|
|
DrawCapsule(position, radius, height, Color.green);
|
|
}
|
|
|
|
private Vector3 Depenetrate(Vector3 position, float radius, float height, float extent)
|
|
{
|
|
Vector3 capsulePoint0 = position + new Vector3(0.0f, radius, 0.0f);
|
|
Vector3 capsulePoint1 = position + new Vector3(0.0f, height - radius, 0.0f);
|
|
|
|
int hitColliderCount = Physics.OverlapCapsuleNonAlloc(capsulePoint0, capsulePoint1, radius + extent, _hitColliders, CollisionLayerMask, QueryTriggerInteraction.Ignore);
|
|
if (hitColliderCount <= 0)
|
|
return position;
|
|
|
|
int remainingSteps = PenetrationSteps;
|
|
while (remainingSteps > 0)
|
|
{
|
|
--remainingSteps;
|
|
|
|
_resolver.Reset();
|
|
|
|
for (int i = 0; i < hitColliderCount; ++i)
|
|
{
|
|
Collider hitCollider = _hitColliders[i];
|
|
if (hitCollider == Collider)
|
|
continue;
|
|
|
|
hitCollider.transform.GetPositionAndRotation(out Vector3 hitPosition, out Quaternion hitRotation);
|
|
|
|
bool hasPenetration = Physics.ComputePenetration(Collider, position, Quaternion.identity, hitCollider, hitPosition, hitRotation, out Vector3 direction, out float distance);
|
|
if (hasPenetration == true)
|
|
{
|
|
_resolver.AddCorrection(direction, distance);
|
|
|
|
if (remainingSteps == PenetrationSteps - 1)
|
|
{
|
|
DrawLine(position, position + direction * distance, Color.red);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (_resolver.Count <= 0)
|
|
break;
|
|
|
|
Vector3 correction = _resolver.CalculateBest(8, 0.0001f);
|
|
correction = Vector3.ClampMagnitude(correction, radius);
|
|
|
|
float correctionMultiplier = Mathf.Max(0.25f, 1.0f - remainingSteps * 0.25f);
|
|
correction *= correctionMultiplier;
|
|
|
|
DrawLine(position, position + correction, Color.yellow);
|
|
position += correction;
|
|
DrawLine(position, position + Vector3.up * 0.1f, Color.magenta);
|
|
}
|
|
|
|
return position;
|
|
}
|
|
|
|
private static void DrawLine(Vector3 fromPosition, Vector3 toPosition, Color color)
|
|
{
|
|
Color gizmosColor = Gizmos.color;
|
|
Gizmos.color = color;
|
|
Gizmos.DrawLine(fromPosition, toPosition);
|
|
Gizmos.color = gizmosColor;
|
|
}
|
|
|
|
private static void DrawCapsule(Vector3 position, float radius, float height, Color color)
|
|
{
|
|
Color gizmosColor = Gizmos.color;
|
|
|
|
Vector3 baseLow = position + Vector3.up * radius;
|
|
Vector3 baseHigh = position + Vector3.up * (height - radius);
|
|
Vector3 offsetFront = Vector3.forward * radius;
|
|
Vector3 offsetBack = Vector3.back * radius;
|
|
Vector3 offsetLeft = Vector3.left * radius;
|
|
Vector3 offsetRight = Vector3.right * radius;
|
|
|
|
Gizmos.color = color;
|
|
|
|
Gizmos.DrawWireSphere(baseLow, radius);
|
|
Gizmos.DrawWireSphere(baseHigh, radius);
|
|
|
|
Gizmos.DrawLine(baseLow + offsetFront, baseHigh + offsetFront);
|
|
Gizmos.DrawLine(baseLow + offsetBack, baseHigh + offsetBack);
|
|
Gizmos.DrawLine(baseLow + offsetLeft, baseHigh + offsetLeft);
|
|
Gizmos.DrawLine(baseLow + offsetRight, baseHigh + offsetRight);
|
|
|
|
Gizmos.color = gizmosColor;
|
|
}
|
|
}
|
|
}
|