private static bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, CapsulePart b, Vector3 offset) { Segment capa, capb = new Segment(b.World.P1, b.World.P2); Vector3.Add(ref a.World.P1, ref offset, out capa.P1); Vector3.Add(ref a.World.P2, ref offset, out capa.P2); Vector3 pa, pb, normal, v; float sa, sb, r2 = a.World.Radius + b.World.Radius; r2 *= r2; // find the closest point between the two capsules Segment.ClosestPoints(ref capa, ref capb, out sa, out pa, out sb, out pb); Vector3.Subtract(ref pa, ref pb, out normal); if (normal.LengthSquared() - r2 >= Constants.Epsilon) return false; if (normal.LengthSquared() < Constants.Epsilon) normal = Vector3.UnitZ; normal.Normalize(); Vector3.Multiply(ref normal, -a.World.Radius, out v); Vector3.Add(ref pa, ref v, out pa); Vector3.Multiply(ref normal, b.World.Radius, out v); Vector3.Add(ref pb, ref v, out pb); Vector3.Subtract(ref pa, ref offset, out pa); cf.WritePoint(ref pa, ref pb, ref normal); // if the two capsules are nearly parallel, an additional support point provides stability if (sa == 0f || sa == 1f) { pa = sa == 0f ? capa.P2 : capa.P1; capb.ClosestPointTo(ref pa, out sa, out pb); } else if (sb == 0f || sb == 1f) { pb = sb == 0f ? capb.P2 : capb.P1; capa.ClosestPointTo(ref pb, out sb, out pa); } else return true; float dist; Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - r2 < Constants.Epsilon) { Vector3.Multiply(ref normal, -a.World.Radius, out v); Vector3.Add(ref pa, ref v, out pa); Vector3.Multiply(ref normal, b.World.Radius, out v); Vector3.Add(ref pb, ref v, out pb); Vector3.Subtract(ref pa, ref offset, out pa); cf.WritePoint(ref pa, ref pb, ref normal); } return true; }
public void Initialize(CollisionFunctor cf, CapsulePart a, MeshPart b, Vector3 offset) { _cf = cf; _b = b; _radius = a.World.Radius * b.TransformInverse.Scale; _radiusSquared = _radius * _radius; _offset = offset; _hasCollision = false; Vector3.Add(ref a.World.P1, ref _offset, out _cap.P1); Vector3.Add(ref a.World.P2, ref _offset, out _cap.P2); // calculate points and bounding box in body space var radius = new Vector3(_radius); Vector3.Transform(ref _cap.P1, ref b.TransformInverse.Combined, out _cap.P1); Vector3.Transform(ref _cap.P2, ref b.TransformInverse.Combined, out _cap.P2); AlignedBox.Fit(ref _cap.P1, ref _cap.P2, out BoundingBox); Vector3.Subtract(ref BoundingBox.Minimum, ref radius, out BoundingBox.Minimum); Vector3.Add(ref BoundingBox.Maximum, ref radius, out BoundingBox.Maximum); }
public HandState() { points = new Vector3[NUM_POINTS]; pointDistanceFromParent = new float[NUM_POINTS]; //for now just doing capsules along bones, but this can change boneCapsules = new CapsulePart[NUM_BONES][]; int gc = 0; for (int b = 0; b < NUM_BONES; b++) { boneCapsules[b] = new CapsulePart[BONE_CAPSULEPART_COUNT[b]]; for (int c = 0; c < boneCapsules[b].Length; c++) { boneCapsules[b][c] = new CapsulePart(); boneCapsules[b][c].idInHand = gc; gc++; } } knuckleOffset = new Vector3[5]; leapReportedBoneAngles = new Quaternion[NUM_BONES]; }
public static bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, PolyhedronPart b, Vector3 offset) { Vector3 v, normal; float ax1, ax2, bx, r2 = a.World.Radius * a.World.Radius; Segment capa; Vector3.Add(ref a.World.P1, ref offset, out capa.P1); Vector3.Add(ref a.World.P2, ref offset, out capa.P2); float curDepth, finalDepth = float.MaxValue; int faceIdx = -1; // find the face with the least penetration depth along its normal for (int i = 0; i < b.FaceCount; i++) { b.World(b.Face(i)[0], out v); b.FaceNormal(i, out normal); Vector3.Dot(ref normal, ref v, out bx); Vector3.Dot(ref normal, ref capa.P1, out ax1); Vector3.Dot(ref normal, ref capa.P2, out ax2); bx += a.World.Radius; curDepth = Math.Max(bx - ax1, bx - ax2); if (curDepth < 0f) { return(false); } if (curDepth < finalDepth) { faceIdx = i; finalDepth = curDepth; } } bool got1 = false, got2 = false; Vector3 pa, pb, pa1, pa2, pb1, pb2; pa1 = pa2 = pb1 = pb2 = Vector3.Zero; var face = b.Face(faceIdx); b.World(face[0], out v); b.FaceNormal(faceIdx, out normal); var plane = new Plane(v, normal); Vector3.Dot(ref normal, ref v, out bx); bx += a.World.Radius; // determine if either capsule point is inside the face pa1 = capa.P1; plane.ClosestPointTo(ref pa1, out pb1); Vector3.Dot(ref normal, ref pa1, out ax1); got1 = ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb1, true); pa2 = capa.P2; plane.ClosestPointTo(ref pa2, out pb2); Vector3.Dot(ref normal, ref pa2, out ax1); if (ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb2, true)) { if (got1) { got2 = true; } else { got1 = true; pa1 = pa2; pb1 = pb2; } } // if one capsule point is inside the face but one is not, try to generate a second point on an edge if (got1 ^ got2) { float sbPrev = float.NaN; int edgePrev = -1; for (int i = 0; i < face.Length; i++) { float dist, sa, sb; Segment edge; b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1); b.World(face[i], out edge.P2); Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb); Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - r2 < Constants.Epsilon && sa >= Constants.Epsilon && sa < 1 - Constants.Epsilon) { if (i == face.Length - 1 && edgePrev == 0 && sb == 1f) { continue; } if (!got2 || (edgePrev == i - 1 && sbPrev == 1f)) { pa2 = pa; pb2 = pb; got2 = true; } sbPrev = sb; edgePrev = i; } } } else if (!got1 && !got2) { // neither point is inside the face, so try all edges float sbPrev = float.NaN, edgePrev = float.NaN; for (int i = 0; i < face.Length; i++) { float dist, sa, sb; Segment edge; b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1); b.World(face[i], out edge.P2); Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb); Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - r2 < Constants.Epsilon && sb > Constants.Epsilon) { if (i == face.Length - 1 && edgePrev == 0 && sb == 1f) { continue; } if (!got1 || (edgePrev == i - 1 && sbPrev == 1f)) { pa1 = pa; pb1 = pb; got1 = true; } else if (!got2) { pa2 = pa; pb2 = pb; got2 = true; } sbPrev = sb; edgePrev = i; } } // if only one edge was crossed, create a new normal instead of using the face normal if (got1 ^ got2) { Vector3.Subtract(ref pa1, ref pb1, out normal); normal.Normalize(); } } // write out points if (got1 || got2) { Vector3.Multiply(ref normal, -a.World.Radius, out v); if (got1) { Vector3.Add(ref pa1, ref v, out pa1); Vector3.Subtract(ref pa1, ref offset, out pa1); cf.WritePoint(ref pa1, ref pb1, ref normal); } if (got2) { Vector3.Add(ref pa2, ref v, out pa2); Vector3.Subtract(ref pa2, ref offset, out pa2); cf.WritePoint(ref pa2, ref pb2, ref normal); } } return(got1 || got2); }
private static bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, CapsulePart b, Vector3 offset) { Segment capa, capb = new Segment(b.World.P1, b.World.P2); Vector3.Add(ref a.World.P1, ref offset, out capa.P1); Vector3.Add(ref a.World.P2, ref offset, out capa.P2); Vector3 pa, pb, normal, v; float sa, sb, r2 = a.World.Radius + b.World.Radius; r2 *= r2; // find the closest point between the two capsules Segment.ClosestPoints(ref capa, ref capb, out sa, out pa, out sb, out pb); Vector3.Subtract(ref pa, ref pb, out normal); if (normal.LengthSquared() - r2 >= Constants.Epsilon) { return(false); } if (normal.LengthSquared() < Constants.Epsilon) { normal = Vector3.UnitZ; } normal.Normalize(); Vector3.Multiply(ref normal, -a.World.Radius, out v); Vector3.Add(ref pa, ref v, out pa); Vector3.Multiply(ref normal, b.World.Radius, out v); Vector3.Add(ref pb, ref v, out pb); Vector3.Subtract(ref pa, ref offset, out pa); cf.WritePoint(ref pa, ref pb, ref normal); // if the two capsules are nearly parallel, an additional support point provides stability if (sa == 0f || sa == 1f) { pa = sa == 0f ? capa.P2 : capa.P1; capb.ClosestPointTo(ref pa, out sa, out pb); } else if (sb == 0f || sb == 1f) { pb = sb == 0f ? capb.P2 : capb.P1; capa.ClosestPointTo(ref pb, out sb, out pa); } else { return(true); } float dist; Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - r2 < Constants.Epsilon) { Vector3.Multiply(ref normal, -a.World.Radius, out v); Vector3.Add(ref pa, ref v, out pa); Vector3.Multiply(ref normal, b.World.Radius, out v); Vector3.Add(ref pb, ref v, out pb); Vector3.Subtract(ref pa, ref offset, out pa); cf.WritePoint(ref pa, ref pb, ref normal); } return(true); }
public static bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, PolyhedronPart b, Vector3 offset) { Vector3 v, normal; float ax1, ax2, bx, r2 = a.World.Radius * a.World.Radius; Segment capa; Vector3.Add(ref a.World.P1, ref offset, out capa.P1); Vector3.Add(ref a.World.P2, ref offset, out capa.P2); float curDepth, finalDepth = float.MaxValue; int faceIdx = -1; // find the face with the least penetration depth along its normal for (int i = 0; i < b.FaceCount; i++) { b.World(b.Face(i)[0], out v); b.FaceNormal(i, out normal); Vector3.Dot(ref normal, ref v, out bx); Vector3.Dot(ref normal, ref capa.P1, out ax1); Vector3.Dot(ref normal, ref capa.P2, out ax2); bx += a.World.Radius; curDepth = Math.Max(bx - ax1, bx - ax2); if (curDepth < 0f) return false; if (curDepth < finalDepth) { faceIdx = i; finalDepth = curDepth; } } bool got1 = false, got2 = false; Vector3 pa, pb, pa1, pa2, pb1, pb2; pa1 = pa2 = pb1 = pb2 = Vector3.Zero; var face = b.Face(faceIdx); b.World(face[0], out v); b.FaceNormal(faceIdx, out normal); var plane = new Plane(v, normal); Vector3.Dot(ref normal, ref v, out bx); bx += a.World.Radius; // determine if either capsule point is inside the face pa1 = capa.P1; plane.ClosestPointTo(ref pa1, out pb1); Vector3.Dot(ref normal, ref pa1, out ax1); got1 = ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb1, true); pa2 = capa.P2; plane.ClosestPointTo(ref pa2, out pb2); Vector3.Dot(ref normal, ref pa2, out ax1); if (ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb2, true)) { if (got1) got2 = true; else { got1 = true; pa1 = pa2; pb1 = pb2; } } // if one capsule point is inside the face but one is not, try to generate a second point on an edge if (got1 ^ got2) { float sbPrev = float.NaN; int edgePrev = -1; for (int i = 0; i < face.Length; i++) { float dist, sa, sb; Segment edge; b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1); b.World(face[i], out edge.P2); Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb); Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - r2 < Constants.Epsilon && sa >= Constants.Epsilon && sa < 1 - Constants.Epsilon) { if (i == face.Length - 1 && edgePrev == 0 && sb == 1f) continue; if (!got2 || (edgePrev == i - 1 && sbPrev == 1f)) { pa2 = pa; pb2 = pb; got2 = true; } sbPrev = sb; edgePrev = i; } } } else if (!got1 && !got2) { // neither point is inside the face, so try all edges float sbPrev = float.NaN, edgePrev = float.NaN; for (int i = 0; i < face.Length; i++) { float dist, sa, sb; Segment edge; b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1); b.World(face[i], out edge.P2); Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb); Vector3.DistanceSquared(ref pa, ref pb, out dist); if (dist - r2 < Constants.Epsilon && sb > Constants.Epsilon) { if (i == face.Length - 1 && edgePrev == 0 && sb == 1f) continue; if (!got1 || (edgePrev == i - 1 && sbPrev == 1f)) { pa1 = pa; pb1 = pb; got1 = true; } else if (!got2) { pa2 = pa; pb2 = pb; got2 = true; } sbPrev = sb; edgePrev = i; } } // if only one edge was crossed, create a new normal instead of using the face normal if (got1 ^ got2) { Vector3.Subtract(ref pa1, ref pb1, out normal); normal.Normalize(); } } // write out points if (got1 || got2) { Vector3.Multiply(ref normal, -a.World.Radius, out v); if (got1) { Vector3.Add(ref pa1, ref v, out pa1); Vector3.Subtract(ref pa1, ref offset, out pa1); cf.WritePoint(ref pa1, ref pb1, ref normal); } if (got2) { Vector3.Add(ref pa2, ref v, out pa2); Vector3.Subtract(ref pa2, ref offset, out pa2); cf.WritePoint(ref pa2, ref pb2, ref normal); } } return got1 || got2; }
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; } }
void Update() { int gc; for (int h = 0; h < 2; h++) { HandState state = source.GetHandState(h == 1); if (state.active) { if (reprs[h, 0] == null) { for (gc = 0; gc < HandState.NUM_CAPSULEPARTS; gc++) { for (int z = 0; z < 2; z++) //do z<3 for back ball { GameObject bon = new GameObject("R"); reprs[h, gc *3 + z] = bon; bon.transform.parent = transform; MeshFilter f = bon.AddComponent <MeshFilter>(); f.mesh = (z == 0 ? cylinder : sphere); MeshRenderer r = bon.AddComponent <MeshRenderer>(); r.material = material; } } } gc = 0; for (int b = 0; b < HandState.NUM_BONES; b++) { Vector3 globalPos = state.GetBonePosition(b); Quaternion globalAng = state.GetBoneAngle(b); for (int c = 0; c < state.boneCapsules[b].Length; c++) { CapsulePart dat = state.boneCapsules[b][c]; Vector3 capsuleGlobalPos = globalPos + globalAng * dat.boneOffsetPos; Quaternion capsuleGlobalAng = globalAng * dat.boneOffsetAng; for (int z = 0; z < 2; z++) { reprs[h, gc *3 + z].transform.rotation = capsuleGlobalAng; MeshRenderer r = reprs[h, gc *3 + z].GetComponent <MeshRenderer>(); r.material = state.notLost ? (system.boneMaterialOverride[h, b] == null ? material : system.boneMaterialOverride[h, b]) : lostMaterial; if (z == 0) { reprs[h, gc * 3 + z].transform.position = capsuleGlobalPos; reprs[h, gc * 3 + z].transform.Rotate(Vector3.right, 90f); //cylinder mesh's axis is along Y instead of Z like it should be reprs[h, gc * 3 + z].transform.localScale = new Vector3(dat.radius * 2.0f, dat.length * 0.5f, dat.radius * 2.0f); } else { reprs[h, gc * 3 + z].transform.position = capsuleGlobalPos + capsuleGlobalAng * Vector3.forward * (dat.length * 0.5f * (z == 1 ? 1.0f : -1.0f)); reprs[h, gc * 3 + z].transform.localScale = new Vector3(dat.radius * 2.0f, dat.radius * 2.0f, dat.radius * 2.0f); } } gc++; } } } else { if (reprs[h, 0] != null) { for (int r = 0; r < HandState.NUM_CAPSULEPARTS * 3; r++) { Destroy(reprs[h, r]); reprs[h, r] = null; } } } } }