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); }