public virtual void InitExtension(TargetConstraint t)
{
if (!Valid(t))
{
return;
}
}
public void RemoveTargetConstraint(TargetConstraint t)
{
if (targets.Contains(t))
{
DisableTarget(t);
targets.Remove(t);
}
}
public override sealed void UpdateExtension(TargetConstraint t)
{
// Linear limit
t.joint.xMotion = ConfigurableJointMotion.Locked;
t.joint.yMotion = ConfigurableJointMotion.Locked;
t.joint.zMotion = ConfigurableJointMotion.Limited;
limitSpring.spring = spring;
limitSpring.damper = damper;
t.joint.linearLimitSpring = limitSpring;
worldLimitStart = t.connectedAnchor.position - t.GetJointAxisWorldRotation() * Vector3.forward * limit.limit;
worldLimitEnd = t.connectedAnchor.position + t.GetJointAxisWorldRotation() * Vector3.forward * limit.limit;
if (showLimitedAxis)
{
Debug.DrawLine(worldLimitStart, worldLimitEnd, Color.black);
}
// Threshold detection
if (detectThreshold)
{
if (togglingThresholdDetectionIn <= 0.0f)
{
closestToLine = BasicHelpers.NearestPointOnFiniteLine(worldLimitStart, worldLimitEnd, t.anchor.position);
if (inverted)
{
positionLerp = Vector3.Distance(worldLimitStart, closestToLine) / (limit.limit * 2.0f);
}
else
{
positionLerp = Vector3.Distance(worldLimitEnd, closestToLine) / (limit.limit * 2.0f);
}
if (positionLerp > threshold && thresholdState != ThresholdState.Over)
{
togglingThresholdDetectionIn = minTimeBetweenDetections;
thresholdState = ThresholdState.Over;
onOverThreshold.Invoke();
}
else if (positionLerp < threshold && thresholdState != ThresholdState.Under)
{
togglingThresholdDetectionIn = minTimeBetweenDetections;
thresholdState = ThresholdState.Under;
onUnderThreshold.Invoke();
}
}
else
{
togglingThresholdDetectionIn -= Time.deltaTime;
}
}
}
public void TeleportToDestination(TargetConstraint t)
{
// Rotate transform until anchor and connAnchor have the same world rotation
Quaternion anchorToDestination = Quaternion.Inverse(t.anchor.rotation) * t.connectedAnchor.rotation;
rb.transform.rotation *= anchorToDestination;
// Move transform until anchor and connAnchor have the same world position
Vector3 displacement = t.connectedAnchor.position - t.anchor.position;
rb.transform.position += displacement;
}
void FixedUpdateJoint(TargetConstraint t)
{
if (editMode && t.joint)
{
// Anchor
t.joint.anchor = t.joint.transform.InverseTransformPoint(t.anchor.position);
// Axis
t.axis.position = t.connectedAnchor.position;
t.axis.rotation = t.GetJointAxisWorldRotation();
}
}
void InstantiateJoint(TargetConstraint t)
{
if (t.joint)
{
return;
}
t.joint = gameObject.AddComponent <ConfigurableJoint>();
t.joint.autoConfigureConnectedAnchor = false;
t.joint.rotationDriveMode = RotationDriveMode.Slerp;
}
bool Valid(TargetConstraint t)
{
if (!t.joint)
{
Debug.LogWarning("Extension cannot be applied. Missing joint");
return(false);
}
if (!t.axis)
{
Debug.LogWarning("Extension cannot be applied. Missing axis");
return(false);
}
return(true);
}
void InstantiateAxis(TargetConstraint t)
{
if (t.axis)
{
return;
}
Transform axis = new GameObject(t.name + ".Axis").transform;
axis.position = t.connectedAnchor.position;
axis.rotation = t.GetJointAxisWorldRotation();
axis.parent = t.connectedAnchor.parent;
t.axis = axis;
}
void InstantiateAnchor(TargetConstraint t)
{
if (t.anchor)
{
return;
}
Transform anchor = new GameObject(t.pheasy.name + ".Anchor").transform;
anchor.position = t.pheasy.transform.position;
anchor.rotation = t.pheasy.transform.rotation;
anchor.parent = t.pheasy.transform;
t.anchor = anchor;
}
bool TargetIsValid(TargetConstraint t)
{
if (!t.connectedAnchor)
{
InstantiateConnectedAnchor(t);
}
if (t.connectedBody && !t.connectedAnchor.IsChildOf(t.connectedBody.transform))
{
Debug.LogWarning("Target " + t.name + " is not valid! Conn anchor is not child of connBody");
return(false);
}
return(true);
}
public sealed override void InitExtension(TargetConstraint t)
{
base.InitExtension(t);
if (!maxDistance)
{
Debug.LogError("Missing reference to Max Distance");
return;
}
t.axis.position = t.connectedAnchor.position;
t.axis.LookAt(maxDistance);
t.setAxisWhenEnabled = true;
t.SetAxis(t.axis.rotation);
limit.limit = Vector3.Distance(t.connectedAnchor.position, maxDistance.position);
t.joint.linearLimit = limit;
}
void InstantiateConnectedAnchor(TargetConstraint t)
{
if (t.connectedAnchor)
{
return;
}
Transform connectedAnchor = new GameObject(t.pheasy.name + ".Goal").transform;
connectedAnchor.position = t.joint.transform.position;
connectedAnchor.rotation = t.joint.transform.rotation;
if (t.connectedBody)
{
connectedAnchor.parent = t.connectedBody.transform;
}
else
{
connectedAnchor.parent = t.joint.transform.parent;
}
t.connectedAnchor = connectedAnchor;
}
void SetStuck(bool stucked, TargetConstraint t)
{
this.stucked = stucked;
if (!stucked && breakableJoint != null)
{
Destroy(breakableJoint);
}
else if (stucked && breakableJoint == null)
{
breakableJoint = t.pheasy.rb.gameObject.AddComponent <ConfigurableJoint>();
breakableJoint.angularZMotion = ConfigurableJointMotion.Locked;
breakableJoint.breakTorque = unstuckingTorque;
}
if (stucked)
{
onStucked.Invoke();
}
else
{
onUnstucked.Invoke();
}
}
public TargetConstraint AddTargetConstraint(string name, Rigidbody connectedBody, bool keepAxisRelativeWithObject, Transform setAxisOnEnable)
{
TargetConstraint newTarget = new TargetConstraint(this, name);
newTarget.connectedBody = connectedBody;
newTarget.keepAxisRelativeToObject = keepAxisRelativeWithObject;
if (setAxisOnEnable)
{
newTarget.axis = setAxisOnEnable;
newTarget.setAxisWhenEnabled = true;
}
else
{
newTarget.setAxisWhenEnabled = false;
}
EnableTarget(newTarget);
targets.Add(newTarget);
return(newTarget);
}
public override sealed void UpdateExtension(TargetConstraint t)
{
if (!t.joint)
{
Debug.LogWarning("Angular limit cannot be applied. Missing joint");
return;
}
if (!t.axis)
{
Debug.LogWarning("Angular limit cannot be applied. Missing axis");
return;
}
// Prevent modification
t.joint.angularXMotion = ConfigurableJointMotion.Locked;
t.joint.angularYMotion = ConfigurableJointMotion.Locked;
t.joint.angularZMotion = angularZMotion;
// Rotation
localRotZ = GetLocalRotZ(t.joint.transform);
// Threshold detection
if (detectThreshold)
{
if (togglingThresholdDetectionIn <= 0.0f)
{
if (localRotZ > threshold && thresholdState != ThresholdState.Over)
{
togglingThresholdDetectionIn = minTimeBetweenDetections;
thresholdState = ThresholdState.Over;
onOverThreshold.Invoke();
}
else if (localRotZ < threshold && thresholdState != ThresholdState.Under)
{
togglingThresholdDetectionIn = minTimeBetweenDetections;
thresholdState = ThresholdState.Under;
onUnderThreshold.Invoke();
}
}
else
{
togglingThresholdDetectionIn -= Time.deltaTime;
}
}
// Stucking
if (canStuck)
{
if (breakableJoint == null && stucked)
{
SetStuck(false, t);
stuckingIn = Random.Range(minRandomDuration, maxRandomDuration);
}
if (!stucked && stuckingIn <= 0.0f)
{
SetStuck(true, t);
}
else
{
stuckingIn -= Time.deltaTime;
}
}
else if (breakableJoint != null)
{
SetStuck(false, t);
}
}
public override void InitExtension(TargetConstraint t)
{
base.InitExtension(t);
}
void UpdateTarget(TargetConstraint t)
{
// Enable / Disable
if (t.enabled && !t.joint)
{
EnableTarget(t);
}
else if (!t.enabled && t.joint)
{
DisableTarget(t);
}
// Update settings
if (t.enabled)
{
// Lerp
if (t.gradualLerp < 1.0f)
{
t.gradualLerp += Time.deltaTime / t.gradualMaxTime;
if (t.gradualLerp >= 1.0f)
{
t.gradualLerp = 1.0f;
}
}
// Joint
UpdateJoint(t, editMode || (!editMode && !t.updated));
// Extensibility
for (int e = 0; e < t.extensions.Count; e++)
{
t.extensions[e].UpdateExtension(t);
}
// Force
if (t.force)
{
TeleportToDestination(t);
}
// Error
t.error = Vector3.Distance(t.anchor.position, t.connectedAnchor.position);
if (t.error < 0.00001f)
{
t.error = 0.0f;
}
}
// Stability
if (safeMode && t.error > maxErrorAllowed)
{
rb.velocity = Vector3.zero;
rb.angularVelocity = Vector3.zero;
}
// Axis
if (t.anchorAxis)
{
if (t.anchorAxis.activeSelf != t.showAnchor)
{
t.anchorAxis.SetActive(t.showAnchor);
}
if (t.showAnchor)
{
UpdateAxis(t.anchorAxis.transform, t.anchor, axisScale);
}
}
if (t.connAnchorAxis)
{
if (t.connAnchorAxis.activeSelf != t.showConnAcnhor)
{
t.connAnchorAxis.SetActive(t.showConnAcnhor);
}
if (t.showConnAcnhor)
{
UpdateAxis(t.connAnchorAxis.transform, t.connectedAnchor, axisScale);
}
}
}
void EnableTarget(TargetConstraint t)
{
if (!t.pheasy)
{
t.pheasy = this;
}
if (!t.joint)
{
InstantiateJoint(t);
}
if (!t.anchor)
{
InstantiateAnchor(t);
}
if (!t.connectedAnchor)
{
InstantiateConnectedAnchor(t);
}
if (t.name == null || t.name == "")
{
t.name = t.anchor.name + "->" + t.connectedAnchor.name;
}
if (t.keepAxisRelativeToObject)
{
t.joint.configuredInWorldSpace = false;
}
else
{
t.joint.configuredInWorldSpace = true;
}
if (!t.axis)
{
t.setAxisWhenEnabled = false;
InstantiateAxis(t);
}
else
{
t.setAxisWhenEnabled = true;
t.SetAxis(t.axis.rotation);
}
if (t.connectedBody)
{
t.joint.connectedBody = t.connectedBody;
t.initialLocalRotation = t.joint.connectedBody.transform.localRotation;
}
t.initialWorldRotation = t.joint.transform.rotation;
t.jointFrameRotation = t.GetJointAxisWorldRotation();
if (axis != null)
{
if (!t.anchorAxis)
{
t.anchorAxis = Instantiate(axis, rb.transform);
UpdateAxis(t.anchorAxis.transform, t.anchor, axisScale);
}
if (!t.connAnchorAxis)
{
t.connAnchorAxis = Instantiate(axis, rb.transform);
UpdateAxis(t.connAnchorAxis.transform, t.connectedAnchor, axisScale);
}
}
t.gradualLerp = 0.0f;
t.extensions.ForEach(e => e.InitExtension(t));
t.enabled = true;
}
void UpdateJoint(TargetConstraint t, bool editMode)
{
if (editMode)
{
// Connected body/anchor
if (t.connectedBody)
{
t.joint.connectedBody = t.connectedBody;
}
else
{
t.joint.connectedBody = null;
}
// Freedom
SetPositionLock(t.joint, t.settings.linearMotion);
SetRotationLock(t.joint, t.settings.angularMotion);
// Drives
if (gradualMode)
{
PhysHelpers.UpdateJointMotionDrive(t.joint, PhysHelpers.JointDriveLerp(new JointDrive(), t.settings.motionDrive.toJointDrive(), t.gradualLerp));
PhysHelpers.UpdateJointAngularDrive(t.joint, PhysHelpers.JointDriveLerp(new JointDrive(), t.settings.angularDrive.toJointDrive(), t.gradualLerp));
}
else
{
PhysHelpers.UpdateJointMotionDrive(t.joint, t.settings.motionDrive.toJointDrive());
PhysHelpers.UpdateJointAngularDrive(t.joint, t.settings.angularDrive.toJointDrive());
}
// Control
t.joint.enableCollision = t.settings.collideWithConnectedRb;
// Prevent modification
t.joint.configuredInWorldSpace = !t.keepAxisRelativeToObject;
}
if (!t.setAxisWhenEnabled)
{
t.joint.autoConfigureConnectedAnchor = false; // Called every frame
}
// Connected body/anchor
if (t.connectedBody)
{
t.tmpConnAnchor = t.joint.connectedBody.transform.InverseTransformPoint(t.connectedAnchor.position);
}
else
{
t.tmpConnAnchor = t.connectedAnchor.position;
}
if (t.joint.connectedAnchor != t.tmpConnAnchor)
{
t.joint.connectedAnchor = t.tmpConnAnchor;
}
// Target rotation
t.jointFrameRotation = t.GetJointAxisWorldRotation();
Quaternion resultRotation = Quaternion.Inverse(t.jointFrameRotation);
resultRotation *= t.anchor.rotation;
resultRotation *= Quaternion.Inverse(t.connectedAnchor.rotation);
resultRotation *= t.jointFrameRotation;
t.joint.targetRotation = resultRotation;
// Target position
if (t.targetPosition)
{
t.tmpTargetPos = -1.0f * t.axis.InverseTransformPoint(t.targetPosition.position);
if (t.connectedBody)
{
t.joint.targetPosition = Vector3.Scale(t.tmpTargetPos, t.connectedBody.transform.localScale);
}
else
{
t.joint.targetPosition = t.tmpTargetPos;
}
}
else if (t.joint.targetPosition != Vector3.zero)
{
t.joint.targetPosition = Vector3.zero;
}
// Update flag
if (!t.updated)
{
t.updated = true;
}
// Debug
if (t.showAxis)
{
Debug.DrawLine(t.axis.position, t.axis.position + t.GetJointAxisWorldRotation() * Vector3.forward * 0.5f, Color.blue);
}
}
void DisableTarget(TargetConstraint t)
{
Destroy(t.joint);
t.enabled = false;
}