// Calculate capsule penetration with a known depenetration direction
public static CapsulePenetrationData TestFixedNormal(Vector3 capsulePos, Quaternion capsuleAng, float capsuleLength, float capsuleRadius, Collider other, Vector3 otherPos, Quaternion otherAng, Vector3 fixedNormal)
{
PrepareTester(capsuleLength, capsuleRadius);
Debug.Assert(fixedNormal.magnitude > 0.99f && fixedNormal.magnitude < 1.01f);
float dist;
Vector3 dir;
float hitDist;
Vector3 hitDir;
if (Physics.ComputePenetration(capsuleCollider, capsulePos, capsuleAng, other, otherPos, otherAng, out dir, out dist))
{
hitDist = dist;
hitDir = dir;
}
else
{
return(null);
}
float loDist = 0.0f;
float hiDist = 0.01f; // default prediction
while (Physics.ComputePenetration(capsuleCollider, capsulePos + hiDist * fixedNormal, capsuleAng, other, otherPos, otherAng, out dir, out dist))
{
hiDist = hiDist * 2.0f;
}
// binary search for depenetration distance
for (int it = 0; it < 8; it++)
{
float midDist = 0.5f * (loDist + hiDist);
if (Physics.ComputePenetration(capsuleCollider, capsulePos + midDist * fixedNormal, capsuleAng, other, otherPos, otherAng, out dir, out dist))
{
loDist = midDist;
hitDist = dist;
hitDir = dir;
}
else
{
hiDist = midDist;
}
}
float dnDot = Vector3.Dot(hitDir, fixedNormal);
if (dnDot > 0f)
{
loDist += dnDot * hitDist;
}
CapsulePenetrationData pen = MakeCapsulePenetrationData(capsulePos, capsuleAng, capsuleLength, capsuleRadius, other, otherPos, otherAng, fixedNormal, loDist);
CleanupTester();
return(pen);
}
// Calculates exactly how the capsule contacted the object at what point on each
private static CapsulePenetrationData MakeCapsulePenetrationData(Vector3 capsulePos, Quaternion capsuleAng, float capsuleLength, float capsuleRadius, Collider other, Vector3 otherPos, Quaternion otherAng, Vector3 dir, float dist)
{
CapsulePenetrationData pen = new CapsulePenetrationData();
Debug.Assert(dir.magnitude > 0.99f && dir.magnitude < 1.01f);
pen.capsuleDepenetrationTranslation = dir * dist;
pen.hitNorm = dir;
float dot = Vector3.Dot(dir, capsuleAng * Vector3.forward);
pen.capsuleHitFraction = dot < 0f ? 1f : 0f;
if (Mathf.Abs(dot) < 0.0001f)
{
// we hit the side of the capsule, not the endpoint
if (other is BoxCollider || other is SphereCollider || other is CapsuleCollider || (other is MeshCollider && (other as MeshCollider).convex))
{
Vector3 projCenter = capsulePos + pen.capsuleDepenetrationTranslation - dir * capsuleRadius;
Vector3 projAxis = 0.5f * capsuleLength * (capsuleAng * Vector3.forward);
float lo = -1f;
float hi = 1f;
Vector3 loAx = projCenter + lo * projAxis;
Vector3 hiAx = projCenter + hi * projAxis;
Vector3 loClose = Physics.ClosestPoint(loAx, other, otherPos, otherAng);
Vector3 hiClose = Physics.ClosestPoint(hiAx, other, otherPos, otherAng);
Vector3 approxHitPos = hiClose;
// binary search
for (int it = 0; it < 8; it++)
{
if ((loAx - loClose).sqrMagnitude < (hiAx - hiClose).sqrMagnitude)
{
hi = lo + (hi - lo) * 0.5f;
hiAx = projCenter + hi * projAxis;
hiClose = Physics.ClosestPoint(hiAx, other, otherPos, otherAng);
approxHitPos = loClose;
}
else
{
lo = hi + (lo - hi) * 0.5f;
loAx = projCenter + lo * projAxis;
loClose = Physics.ClosestPoint(loAx, other, otherPos, otherAng);
approxHitPos = hiClose;
}
}
Vector3 p1 = capsulePos + pen.capsuleDepenetrationTranslation + (-0.5f * capsuleLength * (capsuleAng * Vector3.forward)) - dir * capsuleRadius;
Vector3 p2 = capsulePos + pen.capsuleDepenetrationTranslation + (0.5f * capsuleLength * (capsuleAng * Vector3.forward)) - dir * capsuleRadius;
Vector3 p1ToCenter = approxHitPos - p1;
Vector3 p1ToP2 = p2 - p1;
pen.capsuleHitFraction = Mathf.Clamp01(Vector3.Dot(p1ToP2, p1ToCenter) / p1ToP2.sqrMagnitude);
}
else
{
pen.capsuleHitFraction = 0.5f;
}
}
pen.hitPos = capsulePos + pen.capsuleDepenetrationTranslation + ((pen.capsuleHitFraction - 0.5f) * capsuleLength * (capsuleAng * Vector3.forward)) - dir * capsuleRadius;
return(pen);
}
// Calculate capsule penetration with automatic depenetration direction from Physics.ComputePenetration
public static CapsulePenetrationData Test(Vector3 capsulePos, Quaternion capsuleAng, float capsuleLength, float capsuleRadius, Collider other, Vector3 otherPos, Quaternion otherAng)
{
PrepareTester(capsuleLength, capsuleRadius);
CapsulePenetrationData pen = null;
float dist;
Vector3 dir;
if (Physics.ComputePenetration(capsuleCollider, capsulePos, capsuleAng, other, otherPos, otherAng, out dir, out dist))
{
pen = MakeCapsulePenetrationData(capsulePos, capsuleAng, capsuleLength, capsuleRadius, other, otherPos, otherAng, dir, dist);
}
CleanupTester();
return(pen);
}
public void ProposeContacts(HandState state)
{
lastPos = thisPos;
thisPos = state.GetBonePosition(b);
deltaPos = thisPos - lastPos;
lastRot = thisRot;
thisRot = state.GetBoneAngle(b);
deltaRot = Quaternion.Inverse(lastRot) * thisRot;
contactsAccepted = new Dictionary <GameObject, List <HandContact> >();
for (int c = 0; c < HandState.BONE_CAPSULEPART_COUNT[b]; c++)
{
CapsulePart part = state.boneCapsules[b][c];
capsules[c].transform.localPosition = part.boneOffsetPos;
capsules[c].transform.localRotation = part.boneOffsetAng;
CapsuleCollider capc = capsules[c].GetComponent <CapsuleCollider>();
capc.height = part.length + (part.radius - TrueHandlingPhysics.kinematicBoneRetraction) * 2.0f;
capc.radius = part.radius - TrueHandlingPhysics.kinematicBoneRetraction;
}
kinematicRoot.GetComponent <Rigidbody>().MovePosition(lastPos);
kinematicRoot.GetComponent <Rigidbody>().MoveRotation(lastRot);
main.boneMaterialOverride[h, b] = null;
contactProposals = new Dictionary <GameObject, HandContact[]>();
for (int p = 0; p < HandState.BONE_CAPSULEPART_COUNT[b]; p++)
{
Dictionary <Collider, HandContact> lastPersistence = contactPersistence[p];
Dictionary <Collider, HandContact> nextPersistence = new Dictionary <Collider, HandContact>();
// USING CURRENT RADIUS/LENGTH/OFFSETPOS/OFFSETANG INSTEAD OF LAST, MIGHT BE WRONG BY A TINY BIT
CapsulePart part = state.boneCapsules[b][p];
Vector3 lastCapsulePos = lastPos + (lastRot * part.boneOffsetPos);
Quaternion lastCapsuleRot = lastRot * part.boneOffsetAng;
Vector3 thisCapsulePos = thisPos + (thisRot * part.boneOffsetPos);
Quaternion thisCapsuleRot = thisRot * part.boneOffsetAng;
Vector3 lastOffset = lastCapsuleRot * (Vector3.forward * part.length * 0.5f);
int hits = Physics.OverlapCapsuleNonAlloc(lastCapsulePos + lastOffset, lastCapsulePos - lastOffset, part.radius, main.triggerBuffer, main.handCollisionLayerCollideMask);
for (int i = 0; i < hits; i++)
{
Collider other = main.triggerBuffer[i];
if (other.attachedRigidbody == null)
{
continue;
}
GameObject obj = other.attachedRigidbody == null ? other.gameObject : other.attachedRigidbody.gameObject;
HandContact contact;
if (lastPersistence.ContainsKey(other))
{
//pen = CapsulePenetration.TestFixedNormal(lastCapsulePos, lastCapsuleRot, part.length, part.radius, other, other.transform.position, other.transform.rotation, other.transform.rotation * lastPersistence[other].objectLocalNormal);
contact = lastPersistence[other];
}
else
{
CapsulePenetrationData pen = CapsulePenetration.Test(lastCapsulePos, lastCapsuleRot, part.length, part.radius, other, other.transform.position, other.transform.rotation);
if (pen == null)
{
continue;
}
// If the contact point is inside another capsule, skip it
// This is only running to prevent the contact from being created, but the finger can bend/object can move such that
// an existing contact violates this. To fix that is hard though, because the target point on the capsule surface
// can move when fingers rotate
bool pinched = false;
for (int b2 = 0; b2 < HandState.NUM_BONES; b2++)
{
Vector3 bp = state.GetBonePosition(b2);
Quaternion ba = state.GetBoneAngle(b2);
for (int p2 = 0; p2 < HandState.BONE_CAPSULEPART_COUNT[b2]; p2++)
{
if (b2 == b && p2 == p)
{
continue;
}
CapsulePart otherPart = state.boneCapsules[b2][p2];
Vector3 cp = bp + (ba * otherPart.boneOffsetPos);
Quaternion ca = ba * otherPart.boneOffsetAng;
Vector3 localHit = Quaternion.Inverse(ca) * (pen.hitPos - cp);
// Intersection with the endcap doesn't count, so you can grab with outside of bent fingers
if (localHit.z < -otherPart.length * 0.5f || localHit.z > otherPart.length * 0.5f)
{
continue;
}
Vector3 axPos = new Vector3(0, 0, localHit.z);
if (Vector3.Distance(localHit, axPos) < otherPart.radius)
{
pinched = true;
break;
}
}
if (pinched)
{
break;
}
}
if (pinched)
{
continue;
}
contact = new HandContact();
contact.capsuleAxisFraction = pen.capsuleHitFraction;
contact.objectLocalNormal = Quaternion.Inverse(other.transform.rotation) * pen.hitNorm;
contact.objectSurfaceUnscaledLocalPos = Quaternion.Inverse(other.transform.rotation) * (pen.hitPos - other.transform.position);
contact.objectUnscaledLocalPos = contact.objectSurfaceUnscaledLocalPos + contact.objectLocalNormal * part.radius;
contact.h = h;
contact.b = b;
contact.p = p;
contact.otherCollider = other;
}
//project the capsule pos onto the capsule axis
Vector3 capsuleLocalHitPos = new Vector3(0f, 0f, (contact.capsuleAxisFraction - 0.5f) * part.length);
contact.targetWorldPos = thisCapsulePos + (thisCapsuleRot * capsuleLocalHitPos);
contact.boneRotationDelta = deltaRot;
nextPersistence[other] = contact;
//Gizmoz.DrawLine(pen.hitPos, pen.hitPos - pen.capsuleDepenetrationTranslation, Color.red);
//main.boneMaterialOverride[h, b] = main.touchingMaterial;
HandContact[] contacts;
if (contactProposals.ContainsKey(obj))
{
HandContact[] oldContacts = contactProposals[obj];
contacts = new HandContact[oldContacts.Length + 1];
for (int i2 = 0; i2 < oldContacts.Length; i2++)
{
contacts[i2] = oldContacts[i2];
}
}
else
{
contacts = new HandContact[1];
}
contacts[contacts.Length - 1] = contact;
contactProposals[obj] = contacts;
}
contactPersistence[p] = nextPersistence;
}
}
