public void Initiate(BehaviourBase behaviour, Settings settings, Rigidbody Ibody, Rigidbody[] rigidbodies, ConfigurableJoint joint, Transform[] copPoints, PressureSensor pressureSensor)
{
this.behaviour = behaviour;
this.settings = settings;
this.Ibody = Ibody;
this.rigidbodies = rigidbodies;
this.joint = joint;
this.copPoints = copPoints;
this.pressureSensor = pressureSensor;
toJointSpace = PhysXTools.ToJointSpace(joint);
behaviour.OnPreFixedUpdate += Solve;
}
void Solve(float deltaTime)
{
if (copPoints.Length == 0)
{
cop = joint.transform.TransformPoint(joint.anchor);
}
else
{
cop = Vector3.zero;
foreach (Transform copPoint in copPoints)
{
cop += copPoint.position;
}
cop /= copPoints.Length;
}
cop += settings.copOffset;
com = PhysXTools.GetCenterOfMass(rigidbodies);
comV = PhysXTools.GetCenterOfMassVelocity(rigidbodies);
dir = com - cop;
dirVel = (com + comV * settings.velocityF) - cop;
Vector3 requiredAcceleration = PhysXTools.GetFromToAcceleration(dirVel, -Physics.gravity);
requiredAcceleration -= Ibody.angularVelocity;
Vector3 torque = requiredAcceleration / deltaTime;
PhysXTools.ScaleByInertia(ref torque, Ibody.rotation, Ibody.inertiaTensor * settings.IMlp);
torque = Vector3.ClampMagnitude(torque, settings.maxTorqueMag);
bool pressured = pressureSensor == null || !pressureSensor.enabled || pressureSensor.inContact;
if (pressured)
{
Ibody.AddTorque(torque * settings.torqueMlp, ForceMode.Force);
joint.targetAngularVelocity = Quaternion.Inverse(toJointSpace) * Quaternion.Inverse(joint.transform.rotation) * torque;
}
else
{
joint.targetAngularVelocity = Vector3.zero;
}
}
private void Pin(float pinWeightMaster, float pinPow, float pinDistanceFalloff, bool angularPinning)
{
positionOffset = targetAnimatedCenterOfMass - rigidbody.worldCenterOfMass;
if (float.IsNaN(positionOffset.x))
{
positionOffset = Vector3.zero;
}
float w = pinWeightMaster * pinWeightMlp;
if (w <= 0f)
{
return;
}
w = Mathf.Pow(w, pinPow);
if (Time.deltaTime > 0f)
{
positionOffset /= Time.deltaTime;
}
Vector3 force = -rigidbody.velocity + targetVelocity + positionOffset;
force *= w;
if (pinDistanceFalloff > 0f)
{
force /= 1f + positionOffset.sqrMagnitude * pinDistanceFalloff;
}
rigidbody.AddForce(force, ForceMode.VelocityChange);
// Angular pinning
if (angularPinning)
{
Vector3 torque = PhysXTools.GetAngularAcceleration(rigidbody.rotation, targetAnimatedWorldRotation);
torque -= rigidbody.angularVelocity;
torque *= w;
rigidbody.AddTorque(torque, ForceMode.VelocityChange);
}
}
private void FixedUpdate()
{
Vector3 targetVelocity = Vector3.zero;
Vector3 targetAngularVelocity = Vector3.zero;
// Calculate target velocity and angular velocity
if (useTargetVelocity)
{
targetVelocity = (target.position - lastTargetPos) / Time.deltaTime;
targetAngularVelocity = PhysXTools.GetAngularVelocity(lastTargetRot, target.rotation, Time.deltaTime);
}
lastTargetPos = target.position;
lastTargetRot = target.rotation;
// Force
Vector3 force = PhysXTools.GetLinearAcceleration(r.position, target.position);
force += targetVelocity;
force -= r.velocity;
if (r.useGravity)
{
force -= Physics.gravity * Time.deltaTime;
}
force *= forceWeight;
r.AddForce(force, ForceMode.VelocityChange);
// Torque
Vector3 torque = PhysXTools.GetAngularAcceleration(r.rotation, target.rotation);
torque += targetAngularVelocity;
torque -= r.angularVelocity;
torque *= torqueWeight;
r.AddTorque(torque, ForceMode.VelocityChange);
}
