public PhysicsBoneManager(TrueHandlingPhysics main_, HandState state, int bone_)
{
main = main_;
h = state.right ? 1 : 0;
b = bone_;
Debug.Assert(state.active);
contactProposals = new Dictionary <GameObject, HandContact[]>();
contactPersistence = new Dictionary <Collider, HandContact> [HandState.BONE_CAPSULEPART_COUNT[b]];
noCollided = new HashSet <Collider> [HandState.BONE_CAPSULEPART_COUNT[b]];
for (int p = 0; p < HandState.BONE_CAPSULEPART_COUNT[b]; p++)
{
contactPersistence[p] = new Dictionary <Collider, HandContact>();
noCollided[p] = new HashSet <Collider>();
}
thisPos = state.GetBonePosition(b);
thisRot = state.GetBoneAngle(b);
objects = new GameObject[1];
capsules = new GameObject[HandState.BONE_CAPSULEPART_COUNT[b]];
kinematicRoot = objects[0] = new GameObject("tr");
kinematicRoot.transform.parent = main.transform;
kinematicRoot.layer = TrueHandlingPhysics.handCollisionLayer;
Rigidbody rigid = kinematicRoot.AddComponent <Rigidbody>();
rigid.isKinematic = true;
//rigid.collisionDetectionMode = CollisionDetectionMode.Continuous;
Transform capsuleOwner = kinematicRoot.transform;
for (int i = 0; i < capsules.Length; i++)
{
// THESE DON'T IGNORE COLLISION IN THE JOINT
GameObject cap = capsules[i] = new GameObject(i.ToString());
cap.layer = TrueHandlingPhysics.handCollisionLayer;
cap.AddComponent <CapsuleCollider>().direction = 2;
cap.transform.parent = capsuleOwner;
}
}
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;
}
}
}
}
}