870 lines
33 KiB
C#
870 lines
33 KiB
C#
#define DEBUG_FORCE
|
|
//#define DEBUG_TORQUE
|
|
//#define IMPULSE
|
|
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
using UnityEngine.EventSystems;
|
|
|
|
namespace Passer.Humanoid {
|
|
|
|
public class BasicHandPhysics : MonoBehaviour {
|
|
protected static void DebugLog(string s) {
|
|
#if UNITY_EDITOR
|
|
//Debug.Log(s);
|
|
#endif
|
|
}
|
|
|
|
public HandTarget handTarget;
|
|
|
|
public virtual void Start() {
|
|
if (!handTarget.physics)
|
|
AdvancedHandPhysics.SetKinematic(handTarget.handRigidbody);
|
|
}
|
|
|
|
[HideInInspector]
|
|
public Rigidbody handRigidbody;
|
|
|
|
protected virtual void Initialize() {
|
|
if (handTarget == null)
|
|
return;
|
|
|
|
handRigidbody = GetComponent<Rigidbody>();
|
|
}
|
|
|
|
#region Update
|
|
|
|
private List<Collider> currentTriggers = new List<Collider>();
|
|
private List<Collider> lastTriggers = new List<Collider>();
|
|
|
|
public virtual void FixedUpdate() {
|
|
lastTriggers = currentTriggers;
|
|
currentTriggers = new List<Collider>();
|
|
|
|
}
|
|
|
|
protected virtual void Update() {
|
|
foreach (Collider lastTrigger in lastTriggers) {
|
|
if (!currentTriggers.Contains(lastTrigger))
|
|
SendHandTriggerExit(lastTrigger);
|
|
}
|
|
}
|
|
|
|
public virtual void ManualFixedUpdate(HandTarget _handTarget) {
|
|
handTarget = _handTarget;
|
|
|
|
/*
|
|
Collider[] colliders = Physics.OverlapSphere(handTarget.grabSocket.transform.position, 0.1F);
|
|
if (colliders.Length == 0)
|
|
return;
|
|
|
|
Collider nearingCollider = null;
|
|
for (int i = 0; i < colliders.Length; i++) {
|
|
if (colliders[i].attachedRigidbody == null)
|
|
nearingCollider = colliders[i];
|
|
}
|
|
if (nearingCollider != null)
|
|
HandInteraction.OnNearing(handTarget, nearingCollider.gameObject);
|
|
*/
|
|
if (handRigidbody == null)
|
|
Initialize();
|
|
|
|
if (!handRigidbody.isKinematic)
|
|
UpdateNonKinematicRigidbody();
|
|
}
|
|
|
|
protected virtual void UpdateNonKinematicRigidbody() {
|
|
Vector3 torque = CalculateTorque();
|
|
ApplyTorqueAtPosition(torque, handTarget.handPalm.position);
|
|
|
|
Vector3 wristTorque = CalculateWristTorque();
|
|
ApplyTorqueAtPosition(wristTorque, handTarget.hand.bone.transform.position);
|
|
|
|
Vector3 force = CalculateForce();
|
|
ApplyForceAtPosition(force, handTarget.handPalm.position);
|
|
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
|
#region Events
|
|
|
|
public virtual void OnCollisionEnter(Collision collision) {
|
|
if (collision.contacts.Length == 0)
|
|
return;
|
|
|
|
Rigidbody objRigidbody = collision.rigidbody;
|
|
if (objRigidbody != null)
|
|
handTarget.OnTouchStart(objRigidbody.gameObject, collision.contacts[0].point);
|
|
else
|
|
handTarget.OnTouchStart(collision.gameObject, collision.contacts[0].point);
|
|
}
|
|
|
|
public virtual void OnCollisionExit(Collision collision) {
|
|
Rigidbody objRigidbody = collision.rigidbody;
|
|
if (objRigidbody != null)
|
|
handTarget.OnTouchEnd(objRigidbody.gameObject);
|
|
else
|
|
handTarget.OnTouchEnd(collision.gameObject);
|
|
}
|
|
|
|
public virtual void OnTriggerEnter(Collider collider) {
|
|
if (collider.isTrigger)
|
|
// We cannot touch trigger colliders
|
|
return;
|
|
|
|
Rigidbody objRigidbody = collider.attachedRigidbody;
|
|
if (objRigidbody != null)
|
|
handTarget.touchedObject = objRigidbody.gameObject;
|
|
else
|
|
handTarget.touchedObject = collider.gameObject;
|
|
|
|
handTarget.OnTouchStart(handTarget.touchedObject, collider.transform.position);
|
|
}
|
|
|
|
public virtual void OnTriggerStay(Collider collider) {
|
|
if (!lastTriggers.Contains(collider))
|
|
// This is a new trigger
|
|
SendHandTriggerEnter(collider);
|
|
|
|
currentTriggers.Add(collider);
|
|
|
|
if (collider.isTrigger)
|
|
// We cannot touch trigger colliders
|
|
return;
|
|
|
|
Rigidbody objRigidbody = collider.attachedRigidbody;
|
|
if (objRigidbody != null)
|
|
handTarget.touchedObject = objRigidbody.gameObject;
|
|
else
|
|
handTarget.touchedObject = collider.gameObject;
|
|
|
|
handTarget.GrabCheck(handTarget.touchedObject);
|
|
}
|
|
|
|
public virtual void OnTriggerExit(Collider collider) {
|
|
Rigidbody objRigidbody = collider.attachedRigidbody;
|
|
if (objRigidbody != null)
|
|
handTarget.OnTouchEnd(objRigidbody.gameObject);
|
|
else
|
|
handTarget.OnTouchEnd(collider.gameObject);
|
|
|
|
handTarget.touchedObject = null;
|
|
}
|
|
|
|
protected void SendHandTriggerEnter(Collider collider) {
|
|
Rigidbody objRigidbody = collider.attachedRigidbody;
|
|
|
|
if (collider.gameObject != null) {
|
|
IHandTriggerEvents triggerEvent;
|
|
if (objRigidbody != null)
|
|
triggerEvent = objRigidbody.GetComponent<IHandTriggerEvents>();
|
|
else
|
|
triggerEvent = collider.GetComponent<IHandTriggerEvents>();
|
|
|
|
if (triggerEvent != null)
|
|
triggerEvent.OnHandTriggerEnter(handTarget, collider);
|
|
}
|
|
}
|
|
|
|
protected void SendHandTriggerExit(Collider collider) {
|
|
Rigidbody objRigidbody = collider.attachedRigidbody;
|
|
|
|
if (collider.gameObject != null) {
|
|
IHandTriggerEvents triggerEvent;
|
|
if (objRigidbody != null)
|
|
triggerEvent = objRigidbody.GetComponent<IHandTriggerEvents>();
|
|
else
|
|
triggerEvent = collider.GetComponent<IHandTriggerEvents>();
|
|
|
|
if (triggerEvent != null)
|
|
triggerEvent.OnHandTriggerExit(handTarget, collider);
|
|
}
|
|
}
|
|
#endregion
|
|
|
|
#region Force
|
|
|
|
protected virtual Vector3 CalculateForce() {
|
|
/*
|
|
//Vector3 locationDifference = handTarget.stretchlessTarget.position - handTarget.handRigidbody.position;
|
|
//Debug.DrawLine(handTarget.stretchlessTarget.position, handTarget.handRigidbody.position);
|
|
Vector3 locationDifference = handTarget.transform.position - handTarget.handRigidbody.position;
|
|
//Debug.DrawLine(handTarget.hand.target.transform.position, handTarget.handRigidbody.position);
|
|
|
|
//if (locationDifference.magnitude < 0.01F)
|
|
// return Vector3.zero;
|
|
|
|
Vector3 force = locationDifference * handTarget.strength;
|
|
|
|
force += CalculateForceDamper();
|
|
*/
|
|
Vector3 force = HybridPhysics.CalculateForce(handRigidbody, handTarget.stretchlessTarget.position, handTarget.strength);
|
|
return force;
|
|
}
|
|
|
|
public virtual Vector3 CalculateForce(Vector3 position, Vector3 targetPosition, bool damping = false) {
|
|
Vector3 force = (targetPosition - position) * handTarget.strength;
|
|
if (damping)
|
|
force += CalculateForceDamper();
|
|
return force;
|
|
}
|
|
|
|
private const float damping = 12;
|
|
protected float lastDistanceTime;
|
|
protected Vector3 lastDistanceToTarget;
|
|
protected virtual Vector3 CalculateForceDamper() {
|
|
Vector3 distanceToTarget = handTarget.hand.bone.transform.position - handTarget.hand.target.transform.position;
|
|
|
|
float deltaTime = Time.fixedTime - lastDistanceTime;
|
|
|
|
Vector3 damper = Vector3.zero;
|
|
if (deltaTime < 0.1F) {
|
|
Vector3 velocityTowardsTarget = (distanceToTarget - lastDistanceToTarget) / deltaTime;
|
|
|
|
damper = -velocityTowardsTarget * damping;
|
|
|
|
//Compensate for absolute rigidbody speed (specifically when on a moving platform)
|
|
if (handRigidbody != null) {
|
|
#if UNITY_6000_0_OR_NEWER
|
|
Vector3 residualVelocity = handRigidbody.linearVelocity - velocityTowardsTarget;
|
|
#else
|
|
Vector3 residualVelocity = handRigidbody.velocity - velocityTowardsTarget;
|
|
#endif
|
|
damper += residualVelocity * 10;
|
|
}
|
|
}
|
|
lastDistanceToTarget = distanceToTarget;
|
|
lastDistanceTime = Time.fixedTime;
|
|
|
|
return damper;
|
|
}
|
|
|
|
protected virtual void ApplyForce(Vector3 force) {
|
|
if (float.IsNaN(force.magnitude))
|
|
return;
|
|
|
|
/*
|
|
if (contactPoint.sqrMagnitude > 0) {
|
|
// The contact point is OK, but the force here is not OK, because this is the force from the hand
|
|
// The force needs to be projected on the contactPoint !
|
|
//handRigidbody.AddForceAtPosition(force, contactPoint);
|
|
//#if DEBUG_FORCE
|
|
Debug.DrawRay(contactPoint, force / 10, Color.yellow);
|
|
//#endif
|
|
}
|
|
else {
|
|
// The contact point is OK, but the force here is not OK, because this is the force from the hand
|
|
// The force needs to be projected on the contactPoint !
|
|
//handRigidbody.AddForceAtPosition(force, target.handPalm.position);
|
|
handRigidbody.AddForce(force);
|
|
//#if DEBUG_FORCE
|
|
Debug.DrawRay(target.handPalm.position, force / 10, Color.yellow);
|
|
//#endif
|
|
}
|
|
*/
|
|
handRigidbody.AddForce(force);
|
|
#if DEBUG_FORCE
|
|
Debug.DrawRay(handRigidbody.position, force / 10, Color.yellow);
|
|
#endif
|
|
}
|
|
|
|
protected virtual void ApplyForceAtPosition(Vector3 force, Vector3 position) {
|
|
if (float.IsNaN(force.magnitude))
|
|
return;
|
|
|
|
handRigidbody.AddForceAtPosition(force, position);
|
|
#if DEBUG_FORCE
|
|
Debug.DrawRay(position, force / 10, Color.yellow);
|
|
#endif
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Torque
|
|
|
|
protected virtual Vector3 CalculateTorque() {
|
|
Quaternion sollRotation = handTarget.hand.target.transform.rotation * handTarget.hand.target.toBoneRotation;
|
|
Quaternion istRotation = handTarget.hand.bone.transform.rotation;
|
|
Quaternion dRot = sollRotation * Quaternion.Inverse(istRotation);
|
|
|
|
float angle;
|
|
Vector3 axis;
|
|
dRot.ToAngleAxis(out angle, out axis);
|
|
angle = UnityAngles.Normalize(angle);
|
|
|
|
Vector3 angleDifference = axis.normalized * (angle * Mathf.Deg2Rad);
|
|
Vector3 torque = angleDifference * handTarget.strength * 0.1F;
|
|
return torque;
|
|
}
|
|
|
|
protected virtual Vector3 CalculateWristTorque() {
|
|
//Vector3 wristTension = target.GetWristTension();
|
|
|
|
// Not stable
|
|
//Vector3 forces = new Vector3(-(wristTension.x * wristTension.x * 10), -(wristTension.y * wristTension.y * 10), -wristTension.z * wristTension.z * 10);
|
|
//Vector3 forces = new Vector3(0, -(wristTension.y * wristTension.y * 10), -wristTension.z * wristTension.z * 10);
|
|
|
|
Vector3 torque = Vector3.zero; // (0, 0, -wristTension.z * wristTension.z * 10);
|
|
return torque;
|
|
}
|
|
|
|
private void ApplyTorque(Vector3 torque) {
|
|
//AddTorqueAtPosition(torque, target.handPalm.position);
|
|
ApplyTorqueAtPosition(torque, handTarget.hand.bone.transform.position);
|
|
}
|
|
|
|
protected virtual void ApplyTorqueAtPosition(Vector3 torque, Vector3 posToApply) {
|
|
if (float.IsNaN(torque.magnitude))
|
|
return;
|
|
|
|
Vector3 torqueAxis = torque.normalized;
|
|
Vector3 ortho = new Vector3(1, 0, 0);
|
|
|
|
// prevent torqueAxis and ortho from pointing in the same direction
|
|
if (((torqueAxis - ortho).sqrMagnitude < Mathf.Epsilon) || ((torqueAxis + ortho).sqrMagnitude < Mathf.Epsilon)) {
|
|
ortho = new Vector3(0, 1, 0);
|
|
}
|
|
|
|
ortho = Vector3OrthoNormalize(torqueAxis, ortho);
|
|
// calculate force
|
|
Vector3 force = Vector3.Cross(0.5f * torque, ortho);
|
|
|
|
if (handTarget.handRigidbody != null) {
|
|
handTarget.handRigidbody.AddForceAtPosition(force, posToApply + ortho);
|
|
handTarget.handRigidbody.AddForceAtPosition(-force, posToApply - ortho);
|
|
}
|
|
|
|
#if DEBUG_TORQUE
|
|
UnityEngine.Debug.DrawRay(posToApply + ortho / 20, force / 10, Color.yellow);
|
|
UnityEngine.Debug.DrawLine(posToApply + ortho / 20, posToApply - ortho / 20, Color.yellow);
|
|
UnityEngine.Debug.DrawRay(posToApply - ortho / 20, -force / 10, Color.yellow);
|
|
#endif
|
|
}
|
|
|
|
private Vector3 Vector3OrthoNormalize(Vector3 a, Vector3 b) {
|
|
Vector3 tmp = Vector3.Cross(a.normalized, b).normalized;
|
|
return tmp;
|
|
}
|
|
#endregion
|
|
|
|
}
|
|
|
|
public class AdvancedHandPhysics : BasicHandPhysics {
|
|
#if UNITY_EDITOR
|
|
public static bool debug = false;
|
|
#endif
|
|
public enum PhysicsMode {
|
|
Kinematic,
|
|
NonKinematic,
|
|
HybridKinematic,
|
|
NonKinematicVelocity,
|
|
VelocityUpdate
|
|
}
|
|
public PhysicsMode mode = PhysicsMode.HybridKinematic;
|
|
|
|
protected bool colliding;
|
|
public bool hasCollided = false;
|
|
public Vector3 contactPoint;
|
|
|
|
public Vector3 force;
|
|
public Vector3 torque;
|
|
|
|
protected override void Initialize() {
|
|
if (handTarget == null)
|
|
return;
|
|
|
|
if (enabled) {
|
|
handRigidbody = GetComponent<Rigidbody>();
|
|
if (handRigidbody != null) {
|
|
if (handRigidbody != null) {
|
|
if (handRigidbody.useGravity || mode == PhysicsMode.NonKinematic)
|
|
SetNonKinematic(handRigidbody, handTarget.colliders);
|
|
else
|
|
handTarget.colliders = SetKinematic(handRigidbody);
|
|
}
|
|
handRigidbody.maxAngularVelocity = 20;
|
|
}
|
|
}
|
|
}
|
|
|
|
#region Update
|
|
|
|
public override void FixedUpdate() {
|
|
CalculateVelocity();
|
|
}
|
|
|
|
public override void ManualFixedUpdate(HandTarget _handTarget) {
|
|
handTarget = _handTarget;
|
|
|
|
if (hasCollided && !colliding) {
|
|
handTarget.OnTouchEnd(handTarget.touchedObject);
|
|
handTarget.touchedObject = null;
|
|
}
|
|
|
|
if (handTarget.touchedObject == null) { // Object may be destroyed
|
|
hasCollided = false;
|
|
}
|
|
|
|
if (handRigidbody == null)
|
|
Initialize();
|
|
|
|
// Check for stuck hands. Only when hand is kinematic you can pull the hand loose
|
|
// it will then turn into a kinematic hand, which results in snapping the hand back
|
|
// onto the forearm.
|
|
if (handTarget.forearm.bone.transform != null && handRigidbody != null && !handRigidbody.isKinematic) {
|
|
float distance = Vector3.Distance(handTarget.hand.bone.transform.position, handTarget.forearm.bone.transform.position) - handTarget.forearm.bone.length;
|
|
if (distance > 0.05F) {
|
|
handTarget.colliders = SetKinematic(handRigidbody);
|
|
}
|
|
}
|
|
|
|
UpdateRigidbody();
|
|
|
|
colliding = false;
|
|
}
|
|
|
|
public virtual void UpdateRigidbody() {
|
|
if (handRigidbody == null) {
|
|
UpdateGrabbedMechanicalJoint();
|
|
return;
|
|
}
|
|
|
|
if (mode == PhysicsMode.VelocityUpdate) {
|
|
UpdateRigidbodyVelocity();
|
|
return;
|
|
}
|
|
|
|
if ((mode == PhysicsMode.NonKinematic || mode == PhysicsMode.NonKinematicVelocity) && handRigidbody.isKinematic)
|
|
SetNonKinematic(handRigidbody, handTarget.colliders);
|
|
|
|
Quaternion targetRotation = handTarget.transform.rotation;
|
|
|
|
Quaternion rot = Quaternion.Inverse(handRigidbody.rotation) * targetRotation;
|
|
float angle;
|
|
Vector3 axis;
|
|
rot.ToAngleAxis(out angle, out axis);
|
|
|
|
if (handRigidbody.isKinematic)
|
|
UpdateKinematicRigidbody();
|
|
else
|
|
UpdateNonKinematicRigidbody();
|
|
}
|
|
|
|
protected void UpdateGrabbedMechanicalJoint() {
|
|
Rigidbody grabbedRigidbody = handTarget.hand.bone.transform.GetComponentInParent<Rigidbody>();
|
|
if (grabbedRigidbody == null)
|
|
return;
|
|
|
|
MechanicalJoint grabbedMechanicalJoint = grabbedRigidbody.GetComponent<MechanicalJoint>();
|
|
if (grabbedMechanicalJoint == null)
|
|
return;
|
|
|
|
Vector3 locationDifference = handTarget.hand.target.transform.position - handTarget.hand.bone.transform.position;
|
|
grabbedRigidbody.transform.position += locationDifference;
|
|
|
|
Vector3 correctionVector = grabbedMechanicalJoint.GetCorrectionVector();
|
|
grabbedRigidbody.transform.position += correctionVector;
|
|
|
|
Quaternion rotationDifference = handTarget.hand.target.transform.rotation * Quaternion.Inverse(handTarget.hand.bone.targetRotation);
|
|
grabbedRigidbody.transform.rotation = rotationDifference * grabbedRigidbody.transform.rotation;
|
|
|
|
Quaternion correctionRotation = grabbedMechanicalJoint.GetCorrectionAxisRotation();
|
|
grabbedRigidbody.transform.rotation = grabbedRigidbody.transform.rotation * correctionRotation;
|
|
}
|
|
|
|
protected void UpdateKinematicRigidbody() {
|
|
if (mode == PhysicsMode.NonKinematic ||
|
|
mode == PhysicsMode.HybridKinematic && (
|
|
handRigidbody.mass > HybridPhysics.kinematicMass ||
|
|
handRigidbody.GetComponent<Joint>() != null
|
|
)
|
|
) {
|
|
|
|
SetNonKinematic();
|
|
return;
|
|
}
|
|
|
|
force = Vector3.zero;
|
|
torque = Vector3.zero;
|
|
}
|
|
|
|
protected override void UpdateNonKinematicRigidbody() {
|
|
if (mode != PhysicsMode.NonKinematicVelocity) {
|
|
//torque = CalculateTorque();
|
|
//ApplyTorqueAtPosition(torque, handTarget.handPalm.position);
|
|
|
|
//Vector3 wristTorque = CalculateWristTorque();
|
|
//ApplyTorqueAtPosition(wristTorque, handTarget.hand.bone.transform.position);
|
|
|
|
force = CalculateForce();
|
|
ApplyForceAtPosition(force, handTarget.handPalm.position);
|
|
|
|
// Kinematic Hand Rotation
|
|
handRigidbody.constraints = RigidbodyConstraints.FreezeRotation;
|
|
handTarget.hand.SetBoneRotation(handTarget.hand.target.transform.rotation);
|
|
|
|
if (handTarget.humanoid.haptics && force.magnitude > 5)
|
|
handTarget.Vibrate(force.magnitude / 25);
|
|
}
|
|
else {
|
|
force = Vector3.zero;
|
|
torque = Vector3.zero;
|
|
}
|
|
|
|
if (!hasCollided &&
|
|
!handRigidbody.useGravity &&
|
|
handRigidbody.mass <= HybridPhysics.kinematicMass &&
|
|
mode != PhysicsMode.NonKinematic &&
|
|
mode != PhysicsMode.NonKinematicVelocity &&
|
|
handRigidbody.GetComponent<Joint>() == null
|
|
) {
|
|
|
|
if (!handRigidbody.isKinematic)
|
|
handTarget.colliders = SetKinematic(handRigidbody);
|
|
}
|
|
}
|
|
|
|
private const float MaxVelocityChange = 10f;
|
|
private const float MaxAngularVelocityChange = 20f;
|
|
private const float VelocityMagic = 6000f;
|
|
private const float AngularVelocityMagic = 50f;
|
|
private const float ExpectedDeltaTime = 0.0111f;
|
|
|
|
protected virtual void UpdateRigidbodyVelocity() {
|
|
if (handRigidbody.isKinematic)
|
|
SetNonKinematic();
|
|
|
|
Vector3 handPosition = handRigidbody.transform.position;
|
|
Quaternion handRotation = handRigidbody.transform.rotation * handTarget.hand.bone.toTargetRotation;
|
|
|
|
Vector3 targetPosition = handTarget.hand.target.transform.position;
|
|
Quaternion targetRotation = handTarget.hand.target.transform.rotation;
|
|
|
|
float velocityMagic = VelocityMagic / (Time.fixedDeltaTime / ExpectedDeltaTime);
|
|
float angularVelocityMagic = AngularVelocityMagic / (Time.fixedDeltaTime / ExpectedDeltaTime);
|
|
|
|
Vector3 positionDelta;
|
|
Quaternion rotationDelta;
|
|
|
|
float angle;
|
|
Vector3 axis;
|
|
|
|
positionDelta = (targetPosition - handPosition);
|
|
rotationDelta = targetRotation * Quaternion.Inverse(handRotation);
|
|
|
|
Vector3 velocityTarget = (positionDelta * velocityMagic) * Time.fixedDeltaTime;
|
|
if (float.IsNaN(velocityTarget.x) == false)
|
|
#if UNITY_6000_0_OR_NEWER
|
|
handRigidbody.linearVelocity = Vector3.MoveTowards(handRigidbody.linearVelocity, velocityTarget, MaxVelocityChange);
|
|
#else
|
|
handRigidbody.velocity = Vector3.MoveTowards(handRigidbody.velocity, velocityTarget, MaxVelocityChange);
|
|
#endif
|
|
|
|
rotationDelta.ToAngleAxis(out angle, out axis);
|
|
|
|
if (angle > 180)
|
|
angle -= 360;
|
|
|
|
if (angle != 0) {
|
|
Vector3 angularTarget = angle * axis;
|
|
if (!float.IsNaN(angularTarget.x)) {
|
|
angularTarget = (angularTarget * angularVelocityMagic) * Time.fixedDeltaTime;
|
|
handRigidbody.angularVelocity = Vector3.MoveTowards(handRigidbody.angularVelocity, angularTarget, MaxAngularVelocityChange);
|
|
}
|
|
}
|
|
}
|
|
|
|
#endregion Update
|
|
|
|
#region Events
|
|
|
|
public override void OnTriggerEnter(Collider collider) {
|
|
bool otherHasKinematicPhysics = false;
|
|
bool otherIsHumanoid = false;
|
|
|
|
Rigidbody otherRigidbody = collider.attachedRigidbody;
|
|
if (otherRigidbody != null) {
|
|
AdvancedHandPhysics kp = otherRigidbody.GetComponent<AdvancedHandPhysics>();
|
|
if (kp == null)
|
|
kp = otherRigidbody.GetComponentInParent<AdvancedHandPhysics>();
|
|
otherHasKinematicPhysics = (kp != null);
|
|
HumanoidControl humanoid = otherRigidbody.GetComponent<HumanoidControl>();
|
|
otherIsHumanoid = (humanoid != null);
|
|
}
|
|
|
|
bool collidedWithMyself = IsHumanoidCollider(collider, handTarget.humanoid);
|
|
|
|
if (mode == PhysicsMode.HybridKinematic &&
|
|
handRigidbody != null &&
|
|
handRigidbody.isKinematic &&
|
|
(!collider.isTrigger || otherHasKinematicPhysics) &&
|
|
!otherIsHumanoid &&
|
|
!collidedWithMyself) {
|
|
|
|
colliding = true;
|
|
hasCollided = true;
|
|
if (otherRigidbody != null) {
|
|
handTarget.touchedObject = otherRigidbody.gameObject;
|
|
SetNonKinematic(handRigidbody, handTarget.colliders);
|
|
}
|
|
else {
|
|
handTarget.touchedObject = collider.gameObject;
|
|
SetNonKinematic(handRigidbody, handTarget.colliders);
|
|
}
|
|
|
|
ProcessFirstCollision(handRigidbody, collider);
|
|
}
|
|
|
|
if (hasCollided) {
|
|
if (otherRigidbody != null)
|
|
handTarget.GrabCheck(otherRigidbody.gameObject);
|
|
else
|
|
handTarget.GrabCheck(collider.gameObject);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determine if the collider is part of this humanoid
|
|
/// or attached to this humanoid.
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
/// To be merged with HumanoidControl.IsMyRigidbody ?
|
|
private bool IsHumanoidCollider(Collider collider, HumanoidControl humanoid) {
|
|
Rigidbody otherRigidbody = collider.attachedRigidbody;
|
|
if (otherRigidbody == null)
|
|
return false;
|
|
|
|
// is part of the humanoid?
|
|
if (otherRigidbody == humanoid.leftHandTarget.handRigidbody ||
|
|
otherRigidbody == humanoid.rightHandTarget.handRigidbody ||
|
|
otherRigidbody == humanoid.humanoidRigidbody ||
|
|
otherRigidbody == humanoid.characterRigidbody) {
|
|
return true;
|
|
}
|
|
|
|
/* Why have this here?
|
|
* With this, you cannot touch anything attached to your body (like buttons)
|
|
// is attached to this humanoid?
|
|
if (otherRigidbody.transform.parent == null)
|
|
return false;
|
|
|
|
Rigidbody parentRigidbody = otherRigidbody.transform.parent.GetComponentInParent<Rigidbody>();
|
|
if (parentRigidbody == null)
|
|
return false;
|
|
|
|
if (parentRigidbody == humanoid.leftHandTarget.handRigidbody ||
|
|
parentRigidbody == humanoid.rightHandTarget.handRigidbody ||
|
|
parentRigidbody == humanoid.humanoidRigidbody ||
|
|
parentRigidbody == humanoid.characterRigidbody) {
|
|
return true;
|
|
}
|
|
*/
|
|
return false;
|
|
}
|
|
|
|
public override void OnTriggerExit(Collider collider) {
|
|
}
|
|
|
|
public override void OnCollisionEnter(Collision collision) {
|
|
if (collision.rigidbody != null && collision.rigidbody.gameObject == handTarget.grabbedObject)
|
|
// Don't collide with the things you are holding
|
|
return;
|
|
|
|
colliding = true;
|
|
|
|
if (collision.contacts.Length > 0)
|
|
contactPoint = collision.contacts[0].point;
|
|
base.OnCollisionEnter(collision);
|
|
|
|
// Forward the Collision Enter event to the object the hand is holding
|
|
// This is needed because the rigidbody of the grabbedobject has been disabled
|
|
// Without a rigidbody, the object will no longer receive collision events
|
|
if (handTarget.grabbedRigidbody)
|
|
handTarget.grabbedObject.SendMessage("OnCollisionEnter", collision, SendMessageOptions.DontRequireReceiver);
|
|
}
|
|
|
|
public virtual void OnCollisionStay(Collision collision) {
|
|
if (collision.rigidbody != null && collision.rigidbody.gameObject == handTarget.grabbedObject)
|
|
// Don't collide with the things you are holding
|
|
return;
|
|
|
|
if (collision.rigidbody != null && handTarget.grabbedObject != null && collision.rigidbody.transform.IsChildOf(handTarget.grabbedObject.transform))
|
|
// Don't collide with the things you are holding
|
|
return;
|
|
|
|
colliding = true;
|
|
|
|
if (collision.contacts.Length > 0)
|
|
contactPoint = collision.contacts[0].point;
|
|
}
|
|
|
|
public override void OnCollisionExit(Collision collision) {
|
|
DebugLog("Collision Exit " + collision.gameObject);
|
|
|
|
if (handRigidbody != null && !handRigidbody.useGravity) {
|
|
// The sweeptests fails quite often when holding nested rigidbodies
|
|
// But sweeptest are required to get stable collisions with the environment
|
|
RaycastHit hit;
|
|
if (!handRigidbody.SweepTest(handTarget.transform.position - handRigidbody.position, out hit)) {
|
|
handTarget.OnTouchEnd(handTarget.touchedObject);
|
|
hasCollided = false;
|
|
contactPoint = Vector3.zero;
|
|
handTarget.touchedObject = null;
|
|
}
|
|
}
|
|
|
|
// Forward the Collision Exit event to the object the hand is holding
|
|
// This is needed because the rigidbody of the grabbedobject has been disabled
|
|
// Without a rigidbody, the object will no longer receive collision events
|
|
if (handTarget.grabbedRigidbody)
|
|
handTarget.grabbedObject.SendMessage("OnCollisionExit", collision, SendMessageOptions.DontRequireReceiver);
|
|
}
|
|
|
|
#endregion
|
|
|
|
public void DeterminePhysicsMode(float kinematicMass = 1) {
|
|
mode = DeterminePhysicsMode(handRigidbody, kinematicMass);
|
|
}
|
|
|
|
public static PhysicsMode DeterminePhysicsMode(Rigidbody rigidbody, float kinematicMass = 1) {
|
|
if (rigidbody == null)
|
|
return PhysicsMode.Kinematic;
|
|
|
|
PhysicsMode physicsMode;
|
|
if (rigidbody.useGravity) {
|
|
physicsMode = PhysicsMode.NonKinematic;
|
|
}
|
|
else {
|
|
float mass = CalculateTotalMass(rigidbody);
|
|
if (mass > kinematicMass)
|
|
physicsMode = PhysicsMode.NonKinematic;
|
|
else
|
|
physicsMode = PhysicsMode.HybridKinematic;
|
|
}
|
|
return physicsMode;
|
|
}
|
|
|
|
public static float CalculateTotalMass(Rigidbody rigidbody) {
|
|
if (rigidbody == null)
|
|
return 0;
|
|
|
|
float mass = rigidbody.gameObject.isStatic ? Mathf.Infinity : rigidbody.mass;
|
|
Joint[] joints = rigidbody.GetComponents<Joint>();
|
|
for (int i = 0; i < joints.Length; i++) {
|
|
// Seems to result in cycle in spine in some cases
|
|
//if (joints[i].connectedBody != null)
|
|
// mass += CalculateTotalMass(joints[i].connectedBody);
|
|
//else
|
|
// mass = Mathf.Infinity;
|
|
|
|
// Disabled to support dummy joints to prevent distroying the rigidbodies
|
|
}
|
|
return mass;
|
|
}
|
|
|
|
public Vector3 boneVelocity;
|
|
private Vector3 lastPosition = Vector3.zero;
|
|
private void CalculateVelocity() {
|
|
if (lastPosition != Vector3.zero) {
|
|
boneVelocity = (handTarget.hand.bone.transform.position - lastPosition) / Time.fixedDeltaTime;
|
|
}
|
|
lastPosition = handTarget.hand.bone.transform.position;
|
|
}
|
|
|
|
public void ProcessFirstCollision(Rigidbody rigidbody, Collider otherCollider) {
|
|
|
|
#if IMPULSE
|
|
CalculateCollisionImpuls(rigidbody, otherRigidbody, collisionPoint);
|
|
#endif
|
|
}
|
|
|
|
#if IMPULSE
|
|
private static void CalculateCollisionImpuls(Rigidbody rigidbody, Rigidbody otherRigidbody, Vector3 collisionPoint) {
|
|
if (otherRigidbody != null) {
|
|
Vector3 myImpuls = (rigidbody.mass / 10) * rigidbody.velocity;
|
|
otherRigidbody.AddForceAtPosition(myImpuls, collisionPoint, ForceMode.Impulse);
|
|
}
|
|
}
|
|
#endif
|
|
|
|
public static void SetNonKinematic(Rigidbody rigidbody, List<Collider> colliders) {
|
|
if (rigidbody == null)
|
|
return;
|
|
|
|
DebugLog("SetNonKinematic " + rigidbody.name);
|
|
|
|
GameObject obj = rigidbody.gameObject;
|
|
if (obj.isStatic == false) {
|
|
rigidbody.isKinematic = false;
|
|
Target.UnsetColliderToTrigger(colliders);
|
|
}
|
|
}
|
|
|
|
public static List<Collider> SetKinematic(Rigidbody rigidbody) {
|
|
if (rigidbody == null)
|
|
return null;
|
|
|
|
DebugLog("SetKinematic " + rigidbody.name + " " + rigidbody.isKinematic);
|
|
|
|
GameObject obj = rigidbody.gameObject;
|
|
if (obj.isStatic == false) {
|
|
rigidbody.isKinematic = true;
|
|
return Target.SetColliderToTrigger(obj);
|
|
}
|
|
return null;
|
|
}
|
|
|
|
public void SetNonKinematic() {
|
|
DebugLog("SetNonKinematic");
|
|
|
|
if (handRigidbody == null)
|
|
return;
|
|
|
|
handRigidbody.isKinematic = false;
|
|
UnsetCollidersToTrigger();
|
|
}
|
|
|
|
public void SetKinematic() {
|
|
DebugLog("SetKinematic");
|
|
|
|
if (handRigidbody == null) {
|
|
handTarget.colliders = null;
|
|
return;
|
|
}
|
|
|
|
handRigidbody.isKinematic = true;
|
|
SetCollidersToTrigger();
|
|
}
|
|
|
|
public void SetCollidersToTrigger() {
|
|
DebugLog("SetCollidersToTrigger");
|
|
|
|
List<Collider> changedColliders = handTarget.colliders ?? new List<Collider>();
|
|
|
|
Collider[] thisColliders = handRigidbody.GetComponentsInChildren<Collider>();
|
|
for (int j = 0; j < thisColliders.Length; j++) {
|
|
Rigidbody colliderRigidbody = thisColliders[j].attachedRigidbody;
|
|
if (colliderRigidbody == null || colliderRigidbody == handRigidbody) {
|
|
if (!thisColliders[j].isTrigger) {
|
|
thisColliders[j].isTrigger = true;
|
|
if (!changedColliders.Contains(thisColliders[j]))
|
|
changedColliders.Add(thisColliders[j]);
|
|
}
|
|
}
|
|
}
|
|
handTarget.colliders = changedColliders;
|
|
}
|
|
|
|
public void UnsetCollidersToTrigger() {
|
|
DebugLog("UnsetCollidersToTrigger");
|
|
|
|
if (handTarget.colliders == null)
|
|
return;
|
|
|
|
foreach (Collider c in handTarget.colliders)
|
|
c.isTrigger = false;
|
|
}
|
|
}
|
|
} |