mirror of
https://github.com/BotChain-Robots/ui.git
synced 2026-07-08 15:07:22 +02:00
409
Assets/IK/InverseKinematicsController.cs
Normal file
409
Assets/IK/InverseKinematicsController.cs
Normal file
@@ -0,0 +1,409 @@
|
||||
using UnityEngine;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
|
||||
public class InverseKinematicsController : MonoBehaviour
|
||||
{
|
||||
[Header("IK Settings")]
|
||||
public int maxIterations = 10;
|
||||
public float tolerance = 0.1f;
|
||||
public bool constrainAngles = true;
|
||||
|
||||
[Header("Per-Joint Rotation Deadzone")]
|
||||
[Tooltip("For ServoStraightModule joints only: if CCD computes a rotation smaller than this many degrees for a joint, that joint will not be updated this step.")]
|
||||
[Min(0f)]
|
||||
public float minRotationStepDegreesForDcAndStraight = 10f;
|
||||
|
||||
[Header("Drag Sensitivity")]
|
||||
[Tooltip("How much of the user's mouse drag movement is interpreted (1.0 = 100%, 0.2 = 20%)")]
|
||||
[Range(0.01f, 2.0f)]
|
||||
public float dragSensitivity = 0.7f;
|
||||
|
||||
[Header("Movement Smoothing")]
|
||||
[Range(0.01f, 1f)]
|
||||
public float movementSpeed = 0.3f;
|
||||
public bool useSmoothing = true;
|
||||
|
||||
[Header("Debug")]
|
||||
public bool drawDebugLines = true;
|
||||
public Color debugLineColor = Color.yellow;
|
||||
|
||||
private List<IKJoint> kinematicChain = new List<IKJoint>();
|
||||
private Vector3 targetPosition;
|
||||
private Vector3 smoothedTargetPosition;
|
||||
private Transform endEffector;
|
||||
private Transform anchorPoint;
|
||||
private Vector3 anchorPosition;
|
||||
private bool isSolving = false;
|
||||
|
||||
public bool BuildKinematicChain(Transform targetModule)
|
||||
{
|
||||
kinematicChain.Clear();
|
||||
endEffector = targetModule;
|
||||
|
||||
Transform root = FindTopologyRoot(targetModule);
|
||||
if (root == null || root.name != "GeneratedTopology")
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
BuildChainFromTargetToRoot(targetModule, root);
|
||||
kinematicChain.Reverse();
|
||||
|
||||
if (kinematicChain.Count > 0)
|
||||
{
|
||||
Transform firstBase = kinematicChain[0].baseTransform != null ? kinematicChain[0].baseTransform : kinematicChain[0].pivotTransform;
|
||||
Transform foundAnchor = FindAnchorPoint(firstBase);
|
||||
if (foundAnchor != null)
|
||||
{
|
||||
anchorPoint = foundAnchor;
|
||||
anchorPosition = anchorPoint.position;
|
||||
}
|
||||
else
|
||||
{
|
||||
anchorPoint = firstBase;
|
||||
anchorPosition = anchorPoint.position;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
Transform foundAnchor = FindAnchorPoint(endEffector);
|
||||
if (foundAnchor != null)
|
||||
{
|
||||
anchorPoint = foundAnchor;
|
||||
anchorPosition = anchorPoint.position;
|
||||
}
|
||||
}
|
||||
|
||||
ResetSmoothing();
|
||||
return kinematicChain.Count > 0;
|
||||
}
|
||||
|
||||
private void BuildChainFromTargetToRoot(Transform targetModule, Transform root)
|
||||
{
|
||||
Transform current = targetModule;
|
||||
bool skippedSelectedModuleJoint = false;
|
||||
while (current != null && current != root)
|
||||
{
|
||||
ServoMotorModule servo = current.GetComponentInParent<ServoMotorModule>();
|
||||
if (servo != null && servo.armPivot != null)
|
||||
{
|
||||
if (!kinematicChain.Any(j => j.servo == servo))
|
||||
{
|
||||
if (!skippedSelectedModuleJoint)
|
||||
{
|
||||
skippedSelectedModuleJoint = true;
|
||||
current = current.parent;
|
||||
continue;
|
||||
}
|
||||
IKJoint joint = new IKJoint
|
||||
{
|
||||
servo = servo,
|
||||
pivotTransform = servo.armPivot,
|
||||
baseTransform = servo.transform,
|
||||
minAngle = 0f,
|
||||
maxAngle = 180f,
|
||||
currentAngle = servo.currentAngle
|
||||
};
|
||||
|
||||
// Critical: store axis in pivot-local space; compute world axis each iteration.
|
||||
if (servo is ServoBendModule)
|
||||
{
|
||||
joint.localRotationAxis = Vector3.forward;
|
||||
}
|
||||
else if (servo is ServoStraightModule)
|
||||
{
|
||||
joint.localRotationAxis = Vector3.right;
|
||||
}
|
||||
else
|
||||
{
|
||||
joint.localRotationAxis = Vector3.up;
|
||||
}
|
||||
|
||||
kinematicChain.Add(joint);
|
||||
}
|
||||
}
|
||||
|
||||
current = current.parent;
|
||||
}
|
||||
}
|
||||
|
||||
public bool SolveIK(Vector3 targetPos)
|
||||
{
|
||||
if (kinematicChain.Count == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (useSmoothing)
|
||||
{
|
||||
if (smoothedTargetPosition == Vector3.zero)
|
||||
{
|
||||
smoothedTargetPosition = endEffector != null ? endEffector.position : targetPos;
|
||||
}
|
||||
smoothedTargetPosition = Vector3.Lerp(smoothedTargetPosition, targetPos, movementSpeed);
|
||||
targetPosition = smoothedTargetPosition;
|
||||
}
|
||||
else
|
||||
{
|
||||
targetPosition = targetPos;
|
||||
}
|
||||
|
||||
isSolving = true;
|
||||
|
||||
bool success = SolveCCD();
|
||||
|
||||
isSolving = false;
|
||||
return success;
|
||||
}
|
||||
|
||||
private bool SolveCCD()
|
||||
{
|
||||
for (int iteration = 0; iteration < maxIterations; iteration++)
|
||||
{
|
||||
for (int i = kinematicChain.Count - 1; i >= 0; i--)
|
||||
{
|
||||
IKJoint joint = kinematicChain[i];
|
||||
|
||||
Vector3 endEffectorPos = endEffector.position;
|
||||
|
||||
Vector3 jointPos = joint.pivotTransform.position;
|
||||
|
||||
if (i == 0 && anchorPoint != null)
|
||||
{
|
||||
jointPos = anchorPosition;
|
||||
}
|
||||
|
||||
Vector3 toEndEffector = endEffectorPos - jointPos;
|
||||
Vector3 toTarget = targetPosition - jointPos;
|
||||
|
||||
if (toEndEffector.magnitude < 0.001f || toTarget.magnitude < 0.001f)
|
||||
continue;
|
||||
|
||||
Vector3 worldAxis = joint.pivotTransform.TransformDirection(joint.localRotationAxis).normalized;
|
||||
float angle = Vector3.SignedAngle(toEndEffector, toTarget, worldAxis);
|
||||
|
||||
float desiredAngle = joint.currentAngle + angle;
|
||||
if (constrainAngles)
|
||||
{
|
||||
desiredAngle = Mathf.Clamp(desiredAngle, joint.minAngle, joint.maxAngle);
|
||||
}
|
||||
|
||||
float appliedDelta = desiredAngle - joint.currentAngle;
|
||||
|
||||
bool isStraightServo = joint.servo is ServoStraightModule;
|
||||
if (isStraightServo && Mathf.Abs(appliedDelta) < minRotationStepDegreesForDcAndStraight)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
joint.currentAngle = desiredAngle;
|
||||
joint.servo.SetAngle(desiredAngle);
|
||||
|
||||
endEffectorPos = endEffector.position;
|
||||
|
||||
if (Vector3.Distance(endEffectorPos, targetPosition) < tolerance)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float finalDistance = Vector3.Distance(endEffector.position, targetPosition);
|
||||
return finalDistance < tolerance * 10f;
|
||||
}
|
||||
|
||||
public void ResetSmoothing()
|
||||
{
|
||||
smoothedTargetPosition = Vector3.zero;
|
||||
}
|
||||
|
||||
public Transform GetAnchorPoint()
|
||||
{
|
||||
return anchorPoint;
|
||||
}
|
||||
|
||||
public bool IsAnchorPoint(Transform moduleTransform)
|
||||
{
|
||||
return anchorPoint != null && anchorPoint == moduleTransform;
|
||||
}
|
||||
|
||||
private Transform FindTopologyRoot(Transform module)
|
||||
{
|
||||
Transform current = module;
|
||||
while (current != null)
|
||||
{
|
||||
if (current.name == "GeneratedTopology")
|
||||
{
|
||||
return current;
|
||||
}
|
||||
current = current.parent;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
private Transform FindAnchorPoint(Transform servoTransform)
|
||||
{
|
||||
Transform topologyRoot = FindTopologyRoot(servoTransform);
|
||||
if (topologyRoot == null)
|
||||
{
|
||||
return servoTransform;
|
||||
}
|
||||
|
||||
Transform batteryOrHub = FindBatteryOrHub(topologyRoot);
|
||||
if (batteryOrHub != null)
|
||||
{
|
||||
return batteryOrHub;
|
||||
}
|
||||
|
||||
Transform endModule = FindEndModule(topologyRoot);
|
||||
if (endModule != null)
|
||||
{
|
||||
return endModule;
|
||||
}
|
||||
|
||||
return servoTransform;
|
||||
}
|
||||
|
||||
private Transform FindBatteryOrHub(Transform topologyRoot)
|
||||
{
|
||||
foreach (Transform child in topologyRoot)
|
||||
{
|
||||
ModuleBase module = child.GetComponent<ModuleBase>();
|
||||
if (module != null)
|
||||
{
|
||||
BatteryModule battery = child.GetComponent<BatteryModule>();
|
||||
HubModule hub = child.GetComponent<HubModule>();
|
||||
|
||||
if (battery != null || hub != null)
|
||||
{
|
||||
return child;
|
||||
}
|
||||
}
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
private Transform FindEndModule(Transform topologyRoot)
|
||||
{
|
||||
List<Transform> allModules = new List<Transform>();
|
||||
foreach (Transform child in topologyRoot)
|
||||
{
|
||||
ModuleBase module = child.GetComponent<ModuleBase>();
|
||||
if (module != null)
|
||||
{
|
||||
allModules.Add(child);
|
||||
}
|
||||
}
|
||||
|
||||
foreach (Transform module in allModules)
|
||||
{
|
||||
int connectionCount = CountConnections(module, allModules);
|
||||
|
||||
if (connectionCount == 1)
|
||||
{
|
||||
return module;
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
private int CountConnections(Transform module, List<Transform> allModules)
|
||||
{
|
||||
int count = 0;
|
||||
|
||||
Transform currentParent = module.parent;
|
||||
if (currentParent != null && currentParent.name != "GeneratedTopology")
|
||||
{
|
||||
ModuleBase parentModule = currentParent.GetComponent<ModuleBase>();
|
||||
if (parentModule != null)
|
||||
{
|
||||
count++;
|
||||
}
|
||||
else
|
||||
{
|
||||
Transform checkParent = currentParent.parent;
|
||||
while (checkParent != null && checkParent.name != "GeneratedTopology")
|
||||
{
|
||||
ModuleBase checkModule = checkParent.GetComponent<ModuleBase>();
|
||||
if (checkModule != null)
|
||||
{
|
||||
count++;
|
||||
break;
|
||||
}
|
||||
checkParent = checkParent.parent;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
foreach (Transform otherModule in allModules)
|
||||
{
|
||||
if (otherModule == module) continue;
|
||||
|
||||
Transform checkParent = otherModule.parent;
|
||||
while (checkParent != null && checkParent.name != "GeneratedTopology")
|
||||
{
|
||||
if (checkParent == module)
|
||||
{
|
||||
count++;
|
||||
break;
|
||||
}
|
||||
checkParent = checkParent.parent;
|
||||
}
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
void OnDrawGizmos()
|
||||
{
|
||||
if (!drawDebugLines || kinematicChain.Count == 0)
|
||||
return;
|
||||
|
||||
Gizmos.color = debugLineColor;
|
||||
|
||||
Vector3 prevPos;
|
||||
if (anchorPoint != null)
|
||||
{
|
||||
prevPos = anchorPosition;
|
||||
Gizmos.color = Color.green;
|
||||
Gizmos.DrawSphere(prevPos, 0.15f);
|
||||
Gizmos.color = debugLineColor;
|
||||
}
|
||||
else
|
||||
{
|
||||
prevPos = kinematicChain[0].pivotTransform.position;
|
||||
}
|
||||
|
||||
for (int i = 0; i < kinematicChain.Count; i++)
|
||||
{
|
||||
Vector3 currentPos = kinematicChain[i].pivotTransform.position;
|
||||
Gizmos.DrawLine(prevPos, currentPos);
|
||||
Gizmos.DrawSphere(currentPos, 0.1f);
|
||||
prevPos = currentPos;
|
||||
}
|
||||
|
||||
if (endEffector != null)
|
||||
{
|
||||
Gizmos.DrawLine(prevPos, endEffector.position);
|
||||
Gizmos.DrawSphere(endEffector.position, 0.15f);
|
||||
}
|
||||
|
||||
Gizmos.color = Color.red;
|
||||
Gizmos.DrawSphere(targetPosition, 0.2f);
|
||||
Gizmos.DrawLine(endEffector != null ? endEffector.position : prevPos, targetPosition);
|
||||
}
|
||||
|
||||
private class IKJoint
|
||||
{
|
||||
public ServoMotorModule servo;
|
||||
public Transform pivotTransform;
|
||||
public Transform baseTransform;
|
||||
public Vector3 localRotationAxis;
|
||||
public float minAngle;
|
||||
public float maxAngle;
|
||||
public float currentAngle;
|
||||
}
|
||||
}
|
||||
|
||||
11
Assets/IK/InverseKinematicsController.cs.meta
Normal file
11
Assets/IK/InverseKinematicsController.cs.meta
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: aed8b5469e22847cbab133f3f20750b2
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
585
Assets/IK/ModulePositionController.cs
Normal file
585
Assets/IK/ModulePositionController.cs
Normal file
@@ -0,0 +1,585 @@
|
||||
using UnityEngine;
|
||||
using UnityEngine.EventSystems;
|
||||
using UnityEngine.UI;
|
||||
using System.Collections.Generic;
|
||||
|
||||
public class ModulePositionController : MonoBehaviour
|
||||
{
|
||||
[Header("References")]
|
||||
public Camera mainCamera;
|
||||
public InverseKinematicsController ikController;
|
||||
public LayerMask moduleLayer = -1; // All layers by default
|
||||
|
||||
[Header("Position Control Settings")]
|
||||
public float positionStepSize = 0.1f; // How much to move per button click
|
||||
public float positionChangeThreshold = 0.01f; // Minimum position change to trigger IK solve
|
||||
|
||||
[Header("Visual Feedback")]
|
||||
public Color selectedColor = new Color(0f, 0.8f, 1f, 1f); // Cyan color
|
||||
|
||||
[Header("Selected Module Transparency (IK View)")]
|
||||
[Tooltip("If enabled, the currently selected module becomes transparent in IK view.")]
|
||||
public bool makeSelectedModuleTransparent = true;
|
||||
[Range(0.05f, 1f)]
|
||||
public float selectedModuleAlpha = 0.25f;
|
||||
|
||||
[Header("UI References")]
|
||||
public Button xPlusButton;
|
||||
public Button xMinusButton;
|
||||
public Button yPlusButton;
|
||||
public Button yMinusButton;
|
||||
public Button zPlusButton;
|
||||
public Button zMinusButton;
|
||||
|
||||
[Header("Gizmo")]
|
||||
public PositionControlGizmo gizmo;
|
||||
|
||||
[HideInInspector] public ModuleBase selectedModule;
|
||||
private Vector3 targetPosition;
|
||||
private Vector3 lastSuccessfulPosition;
|
||||
private bool[] axisLimitReached = new bool[3];
|
||||
private int[] axisLimitDirection = new int[3];
|
||||
private bool gizmoWasClicked = false;
|
||||
|
||||
// Critical: modules are parented; apply transparency only to renderers owned by the selected module.
|
||||
private readonly Dictionary<Renderer, Material[]> _originalSharedMaterialsByRenderer = new();
|
||||
private readonly List<Renderer> _transparentRenderers = new();
|
||||
|
||||
void Start()
|
||||
{
|
||||
if (mainCamera == null)
|
||||
{
|
||||
mainCamera = Camera.main;
|
||||
if (mainCamera == null)
|
||||
{
|
||||
mainCamera = FindObjectOfType<Camera>();
|
||||
}
|
||||
}
|
||||
|
||||
if (ikController == null)
|
||||
{
|
||||
ikController = GetComponentInParent<InverseKinematicsController>();
|
||||
if (ikController == null)
|
||||
{
|
||||
ikController = FindObjectOfType<InverseKinematicsController>();
|
||||
}
|
||||
}
|
||||
|
||||
if (gizmo == null)
|
||||
{
|
||||
gizmo = GetComponentInChildren<PositionControlGizmo>();
|
||||
if (gizmo == null)
|
||||
{
|
||||
gizmo = FindObjectOfType<PositionControlGizmo>();
|
||||
}
|
||||
if (gizmo != null && gizmo.positionController == null)
|
||||
{
|
||||
gizmo.positionController = this;
|
||||
}
|
||||
}
|
||||
|
||||
SetupUIButtons();
|
||||
}
|
||||
|
||||
void SetupUIButtons()
|
||||
{
|
||||
if (xPlusButton != null) xPlusButton.onClick.RemoveAllListeners();
|
||||
if (xMinusButton != null) xMinusButton.onClick.RemoveAllListeners();
|
||||
if (yPlusButton != null) yPlusButton.onClick.RemoveAllListeners();
|
||||
if (yMinusButton != null) yMinusButton.onClick.RemoveAllListeners();
|
||||
if (zPlusButton != null) zPlusButton.onClick.RemoveAllListeners();
|
||||
if (zMinusButton != null) zMinusButton.onClick.RemoveAllListeners();
|
||||
|
||||
if (xPlusButton != null)
|
||||
{
|
||||
xPlusButton.onClick.AddListener(() => AdjustPositionAxis(0, 1));
|
||||
}
|
||||
|
||||
if (xMinusButton != null)
|
||||
{
|
||||
xMinusButton.onClick.AddListener(() => AdjustPositionAxis(0, -1));
|
||||
}
|
||||
|
||||
if (yPlusButton != null)
|
||||
{
|
||||
yPlusButton.onClick.AddListener(() => AdjustPositionAxis(1, 1));
|
||||
}
|
||||
|
||||
if (yMinusButton != null)
|
||||
{
|
||||
yMinusButton.onClick.AddListener(() => AdjustPositionAxis(1, -1));
|
||||
}
|
||||
|
||||
if (zPlusButton != null)
|
||||
{
|
||||
zPlusButton.onClick.AddListener(() => AdjustPositionAxis(2, 1));
|
||||
}
|
||||
|
||||
if (zMinusButton != null)
|
||||
{
|
||||
zMinusButton.onClick.AddListener(() => AdjustPositionAxis(2, -1));
|
||||
}
|
||||
}
|
||||
|
||||
public void AdjustPositionAxis(int axis, int sign)
|
||||
{
|
||||
if (selectedModule == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (ikController == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3 currentActualPosition = selectedModule.transform.position;
|
||||
|
||||
// Critical: preserve user intent on the other axes when adjusting only one axis.
|
||||
float targetOtherAxisX = targetPosition.x;
|
||||
float targetOtherAxisY = targetPosition.y;
|
||||
float targetOtherAxisZ = targetPosition.z;
|
||||
|
||||
Vector3 positionBeforeIK = currentActualPosition;
|
||||
|
||||
Vector3 newTargetPosition = targetPosition;
|
||||
|
||||
switch (axis)
|
||||
{
|
||||
case 0: // X axis
|
||||
newTargetPosition.x += sign * positionStepSize;
|
||||
break;
|
||||
case 1: // Y axis
|
||||
newTargetPosition.y += sign * positionStepSize;
|
||||
break;
|
||||
case 2: // Z axis
|
||||
newTargetPosition.z += sign * positionStepSize;
|
||||
break;
|
||||
}
|
||||
|
||||
float currentAxisValue = 0f;
|
||||
float newAxisValue = 0f;
|
||||
switch (axis)
|
||||
{
|
||||
case 0:
|
||||
currentAxisValue = positionBeforeIK.x;
|
||||
newAxisValue = newTargetPosition.x;
|
||||
break;
|
||||
case 1:
|
||||
currentAxisValue = positionBeforeIK.y;
|
||||
newAxisValue = newTargetPosition.y;
|
||||
break;
|
||||
case 2:
|
||||
currentAxisValue = positionBeforeIK.z;
|
||||
newAxisValue = newTargetPosition.z;
|
||||
break;
|
||||
}
|
||||
|
||||
targetPosition = newTargetPosition;
|
||||
|
||||
if (ikController != null)
|
||||
{
|
||||
ikController.ResetSmoothing();
|
||||
}
|
||||
|
||||
bool success = false;
|
||||
if (ikController != null)
|
||||
{
|
||||
success = ikController.SolveIK(targetPosition);
|
||||
}
|
||||
|
||||
Vector3 positionAfterIK = selectedModule.transform.position;
|
||||
|
||||
float axisMovement = 0f;
|
||||
float axisValueBefore = 0f;
|
||||
float axisValueAfter = 0f;
|
||||
|
||||
switch (axis)
|
||||
{
|
||||
case 0: // X axis
|
||||
axisValueBefore = positionBeforeIK.x;
|
||||
axisValueAfter = positionAfterIK.x;
|
||||
axisMovement = Mathf.Abs(axisValueAfter - axisValueBefore);
|
||||
break;
|
||||
case 1: // Y axis
|
||||
axisValueBefore = positionBeforeIK.y;
|
||||
axisValueAfter = positionAfterIK.y;
|
||||
axisMovement = Mathf.Abs(axisValueAfter - axisValueBefore);
|
||||
break;
|
||||
case 2: // Z axis
|
||||
axisValueBefore = positionBeforeIK.z;
|
||||
axisValueAfter = positionAfterIK.z;
|
||||
axisMovement = Mathf.Abs(axisValueAfter - axisValueBefore);
|
||||
break;
|
||||
}
|
||||
|
||||
bool movementInCorrectDirection = false;
|
||||
switch (axis)
|
||||
{
|
||||
case 0: // X axis
|
||||
movementInCorrectDirection = (sign > 0 && axisValueAfter > axisValueBefore) || (sign < 0 && axisValueAfter < axisValueBefore);
|
||||
break;
|
||||
case 1: // Y axis
|
||||
movementInCorrectDirection = (sign > 0 && axisValueAfter > axisValueBefore) || (sign < 0 && axisValueAfter < axisValueBefore);
|
||||
break;
|
||||
case 2: // Z axis
|
||||
movementInCorrectDirection = (sign > 0 && axisValueAfter > axisValueBefore) || (sign < 0 && axisValueAfter < axisValueBefore);
|
||||
break;
|
||||
}
|
||||
|
||||
// Critical: prevent target drift when constraints move opposite the user's intent.
|
||||
bool shouldUpdateTarget = (success || axisMovement >= positionChangeThreshold) && movementInCorrectDirection;
|
||||
Vector3 updatedTargetPosition = targetPosition;
|
||||
|
||||
if (shouldUpdateTarget)
|
||||
{
|
||||
updatedTargetPosition = targetPosition;
|
||||
|
||||
switch (axis)
|
||||
{
|
||||
case 0:
|
||||
updatedTargetPosition.x = positionAfterIK.x;
|
||||
updatedTargetPosition.y = targetOtherAxisY;
|
||||
updatedTargetPosition.z = targetOtherAxisZ;
|
||||
break;
|
||||
case 1:
|
||||
updatedTargetPosition.x = targetOtherAxisX;
|
||||
updatedTargetPosition.y = positionAfterIK.y;
|
||||
updatedTargetPosition.z = targetOtherAxisZ;
|
||||
break;
|
||||
case 2:
|
||||
updatedTargetPosition.x = targetOtherAxisX;
|
||||
updatedTargetPosition.y = targetOtherAxisY;
|
||||
updatedTargetPosition.z = positionAfterIK.z;
|
||||
break;
|
||||
}
|
||||
|
||||
targetPosition = updatedTargetPosition;
|
||||
}
|
||||
else
|
||||
{
|
||||
switch (axis)
|
||||
{
|
||||
case 0:
|
||||
targetPosition.x = targetOtherAxisX;
|
||||
break;
|
||||
case 1:
|
||||
targetPosition.y = targetOtherAxisY;
|
||||
break;
|
||||
case 2:
|
||||
targetPosition.z = targetOtherAxisZ;
|
||||
break;
|
||||
}
|
||||
updatedTargetPosition = targetPosition;
|
||||
|
||||
if (axisMovement >= positionChangeThreshold && !movementInCorrectDirection)
|
||||
{
|
||||
if (ikController != null)
|
||||
{
|
||||
ikController.ResetSmoothing();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
lastSuccessfulPosition = positionAfterIK;
|
||||
axisLimitReached[axis] = false;
|
||||
axisLimitDirection[axis] = 0;
|
||||
|
||||
}
|
||||
|
||||
void LateUpdate()
|
||||
{
|
||||
HandleSelection();
|
||||
}
|
||||
|
||||
void HandleSelection()
|
||||
{
|
||||
if (EventSystem.current != null && EventSystem.current.IsPointerOverGameObject())
|
||||
return;
|
||||
|
||||
if (Input.GetMouseButtonDown(0))
|
||||
{
|
||||
gizmoWasClicked = false;
|
||||
|
||||
if (gizmo != null && gizmo.enabled && gizmo.WasGizmoClicked())
|
||||
{
|
||||
gizmoWasClicked = true;
|
||||
return; // Don't process selection/deselection when gizmo arrow is clicked
|
||||
}
|
||||
|
||||
if (gizmoWasClicked)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (mainCamera == null)
|
||||
{
|
||||
mainCamera = Camera.main;
|
||||
if (mainCamera == null)
|
||||
{
|
||||
mainCamera = FindObjectOfType<Camera>();
|
||||
}
|
||||
if (mainCamera == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
Ray ray = mainCamera.ScreenPointToRay(Input.mousePosition);
|
||||
RaycastHit hit;
|
||||
|
||||
if (Physics.Raycast(ray, out hit, Mathf.Infinity, moduleLayer))
|
||||
{
|
||||
ModuleBase hitModule = hit.collider.GetComponentInParent<ModuleBase>();
|
||||
|
||||
if (hitModule != null && IsPartOfGeneratedTopology(hitModule.transform))
|
||||
{
|
||||
SelectModule(hitModule);
|
||||
}
|
||||
else
|
||||
{
|
||||
DeselectModule();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!gizmoWasClicked)
|
||||
{
|
||||
DeselectModule();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void OnGizmoClicked()
|
||||
{
|
||||
gizmoWasClicked = true;
|
||||
}
|
||||
|
||||
void SelectModule(ModuleBase module)
|
||||
{
|
||||
if (selectedModule != null && selectedModule != module)
|
||||
{
|
||||
DeselectModule();
|
||||
}
|
||||
|
||||
ServoMotorModule.selectedModule = null;
|
||||
ObjectSelector.selectedObject = null;
|
||||
|
||||
selectedModule = module;
|
||||
targetPosition = selectedModule.transform.position;
|
||||
lastSuccessfulPosition = selectedModule.transform.position;
|
||||
axisLimitReached[0] = false;
|
||||
axisLimitReached[1] = false;
|
||||
axisLimitReached[2] = false;
|
||||
axisLimitDirection[0] = 0;
|
||||
axisLimitDirection[1] = 0;
|
||||
axisLimitDirection[2] = 0;
|
||||
|
||||
ServoMotorModule servoModule = module.GetComponent<ServoMotorModule>();
|
||||
if (servoModule != null)
|
||||
{
|
||||
ServoMotorModule.selectedModule = servoModule;
|
||||
}
|
||||
else
|
||||
{
|
||||
ObjectSelector.selectedObject = module.gameObject;
|
||||
}
|
||||
|
||||
if (gizmo != null)
|
||||
{
|
||||
gizmo.enabled = true;
|
||||
gizmo.RefreshGizmoVisibility();
|
||||
}
|
||||
|
||||
if (ikController != null)
|
||||
{
|
||||
bool chainBuilt = ikController.BuildKinematicChain(selectedModule.transform);
|
||||
|
||||
if (chainBuilt)
|
||||
{
|
||||
if (ikController.IsAnchorPoint(selectedModule.transform))
|
||||
{
|
||||
DeselectModule();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
ikController.ResetSmoothing();
|
||||
}
|
||||
|
||||
if (makeSelectedModuleTransparent)
|
||||
{
|
||||
ApplyTransparencyToSelectedModule();
|
||||
}
|
||||
}
|
||||
|
||||
public void DeselectModule()
|
||||
{
|
||||
RestoreTransparency();
|
||||
|
||||
if (selectedModule != null)
|
||||
{
|
||||
ServoMotorModule.selectedModule = null;
|
||||
ObjectSelector.selectedObject = null;
|
||||
}
|
||||
|
||||
selectedModule = null;
|
||||
|
||||
if (gizmo != null)
|
||||
{
|
||||
gizmo.enabled = false;
|
||||
gizmo.RefreshGizmoVisibility();
|
||||
}
|
||||
}
|
||||
|
||||
private void ApplyTransparencyToSelectedModule()
|
||||
{
|
||||
RestoreTransparency();
|
||||
|
||||
if (selectedModule == null) return;
|
||||
|
||||
// Critical: don't descend into other ModuleBase subtrees (child modules).
|
||||
foreach (var r in GetRenderersOwnedByModule(selectedModule))
|
||||
{
|
||||
if (!_originalSharedMaterialsByRenderer.ContainsKey(r))
|
||||
{
|
||||
_originalSharedMaterialsByRenderer[r] = r.sharedMaterials;
|
||||
}
|
||||
|
||||
Material[] mats = r.materials;
|
||||
for (int i = 0; i < mats.Length; i++)
|
||||
{
|
||||
var m = mats[i];
|
||||
if (m == null) continue;
|
||||
ConfigureMaterialForTransparency(m, selectedModuleAlpha);
|
||||
}
|
||||
|
||||
_transparentRenderers.Add(r);
|
||||
}
|
||||
}
|
||||
|
||||
private static IEnumerable<Renderer> GetRenderersOwnedByModule(ModuleBase rootModule)
|
||||
{
|
||||
if (rootModule == null) yield break;
|
||||
|
||||
Transform root = rootModule.transform;
|
||||
var stack = new Stack<Transform>();
|
||||
stack.Push(root);
|
||||
|
||||
while (stack.Count > 0)
|
||||
{
|
||||
Transform t = stack.Pop();
|
||||
if (t == null) continue;
|
||||
|
||||
if (t != root)
|
||||
{
|
||||
ModuleBase other = t.GetComponent<ModuleBase>();
|
||||
if (other != null && other != rootModule)
|
||||
continue;
|
||||
}
|
||||
|
||||
foreach (var r in t.GetComponents<Renderer>())
|
||||
{
|
||||
if (r != null)
|
||||
yield return r;
|
||||
}
|
||||
|
||||
for (int i = 0; i < t.childCount; i++)
|
||||
{
|
||||
stack.Push(t.GetChild(i));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void ConfigureMaterialForTransparency(Material m, float alpha)
|
||||
{
|
||||
alpha = Mathf.Clamp01(alpha);
|
||||
|
||||
if (m.HasProperty("_Surface"))
|
||||
{
|
||||
m.SetFloat("_Surface", 1f);
|
||||
|
||||
if (m.HasProperty("_Blend")) m.SetFloat("_Blend", 0f);
|
||||
if (m.HasProperty("_AlphaClip")) m.SetFloat("_AlphaClip", 0f);
|
||||
|
||||
m.EnableKeyword("_SURFACE_TYPE_TRANSPARENT");
|
||||
m.DisableKeyword("_SURFACE_TYPE_OPAQUE");
|
||||
|
||||
m.SetOverrideTag("RenderType", "Transparent");
|
||||
if (m.HasProperty("_SrcBlend")) m.SetFloat("_SrcBlend", (float)UnityEngine.Rendering.BlendMode.SrcAlpha);
|
||||
if (m.HasProperty("_DstBlend")) m.SetFloat("_DstBlend", (float)UnityEngine.Rendering.BlendMode.OneMinusSrcAlpha);
|
||||
if (m.HasProperty("_ZWrite")) m.SetFloat("_ZWrite", 0f);
|
||||
m.renderQueue = (int)UnityEngine.Rendering.RenderQueue.Transparent;
|
||||
}
|
||||
else if (m.HasProperty("_Mode"))
|
||||
{
|
||||
m.SetFloat("_Mode", 3f);
|
||||
m.SetOverrideTag("RenderType", "Transparent");
|
||||
if (m.HasProperty("_SrcBlend")) m.SetFloat("_SrcBlend", (float)UnityEngine.Rendering.BlendMode.SrcAlpha);
|
||||
if (m.HasProperty("_DstBlend")) m.SetFloat("_DstBlend", (float)UnityEngine.Rendering.BlendMode.OneMinusSrcAlpha);
|
||||
if (m.HasProperty("_ZWrite")) m.SetFloat("_ZWrite", 0f);
|
||||
m.DisableKeyword("_ALPHATEST_ON");
|
||||
m.EnableKeyword("_ALPHABLEND_ON");
|
||||
m.DisableKeyword("_ALPHAPREMULTIPLY_ON");
|
||||
m.renderQueue = (int)UnityEngine.Rendering.RenderQueue.Transparent;
|
||||
}
|
||||
else
|
||||
{
|
||||
m.SetOverrideTag("RenderType", "Transparent");
|
||||
if (m.HasProperty("_SrcBlend")) m.SetFloat("_SrcBlend", (float)UnityEngine.Rendering.BlendMode.SrcAlpha);
|
||||
if (m.HasProperty("_DstBlend")) m.SetFloat("_DstBlend", (float)UnityEngine.Rendering.BlendMode.OneMinusSrcAlpha);
|
||||
if (m.HasProperty("_ZWrite")) m.SetFloat("_ZWrite", 0f);
|
||||
m.renderQueue = (int)UnityEngine.Rendering.RenderQueue.Transparent;
|
||||
}
|
||||
|
||||
if (m.HasProperty("_BaseColor"))
|
||||
{
|
||||
Color c = m.GetColor("_BaseColor");
|
||||
c.a = alpha;
|
||||
m.SetColor("_BaseColor", c);
|
||||
}
|
||||
if (m.HasProperty("_Color"))
|
||||
{
|
||||
Color c = m.GetColor("_Color");
|
||||
c.a = alpha;
|
||||
m.SetColor("_Color", c);
|
||||
}
|
||||
}
|
||||
|
||||
private void RestoreTransparency()
|
||||
{
|
||||
if (_transparentRenderers.Count == 0 && _originalSharedMaterialsByRenderer.Count == 0) return;
|
||||
|
||||
foreach (var r in _transparentRenderers)
|
||||
{
|
||||
if (r == null) continue;
|
||||
if (_originalSharedMaterialsByRenderer.TryGetValue(r, out var shared))
|
||||
{
|
||||
r.sharedMaterials = shared;
|
||||
}
|
||||
}
|
||||
|
||||
_transparentRenderers.Clear();
|
||||
_originalSharedMaterialsByRenderer.Clear();
|
||||
}
|
||||
|
||||
bool IsPartOfGeneratedTopology(Transform moduleTransform)
|
||||
{
|
||||
Transform current = moduleTransform;
|
||||
while (current != null)
|
||||
{
|
||||
if (current.name == "GeneratedTopology")
|
||||
{
|
||||
return true;
|
||||
}
|
||||
current = current.parent;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void OnDisable()
|
||||
{
|
||||
DeselectModule();
|
||||
}
|
||||
}
|
||||
11
Assets/IK/ModulePositionController.cs.meta
Normal file
11
Assets/IK/ModulePositionController.cs.meta
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: d35bf7a5ed4da4d708a643ace1b67073
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
854
Assets/IK/PositionControlGizmo.cs
Normal file
854
Assets/IK/PositionControlGizmo.cs
Normal file
@@ -0,0 +1,854 @@
|
||||
using UnityEngine;
|
||||
using UnityEngine.EventSystems;
|
||||
using UnityEngine.UI;
|
||||
using TMPro;
|
||||
public class PositionControlGizmo : MonoBehaviour
|
||||
{
|
||||
[Header("Gizmo Settings")]
|
||||
public float arrowLength = 1.0f;
|
||||
public float arrowHeadSize = 0.25f;
|
||||
public float lineWidth = 0.04f;
|
||||
public float clickableRadius = 0.5f;
|
||||
public float screenSpaceClickThreshold = 30f;
|
||||
|
||||
[Header("Colors")]
|
||||
public Color xAxisColor = Color.red;
|
||||
public Color yAxisColor = Color.green;
|
||||
public Color zAxisColor = Color.blue;
|
||||
public Color hoverColor = Color.yellow;
|
||||
public Color selectedColor = new Color(1f, 1f, 0f, 0.8f);
|
||||
|
||||
[Header("References")]
|
||||
public Camera mainCamera;
|
||||
public ModulePositionController positionController;
|
||||
|
||||
[Header("Placement")]
|
||||
[Tooltip("If enabled, the gizmo is drawn at the selected module's visual center (renderer/collider bounds center), rather than transform.position.")]
|
||||
public bool centerOnModuleBounds = true;
|
||||
[Tooltip("World-space offset added to gizmo origin.")]
|
||||
public Vector3 gizmoWorldOffset = Vector3.zero;
|
||||
|
||||
private ModuleBase targetModule;
|
||||
private int hoveredAxis = -1;
|
||||
private int previousHoveredAxis = -1;
|
||||
private int selectedAxis = -1;
|
||||
private Color[] previousColors = new Color[3];
|
||||
private bool isDragging = false;
|
||||
private bool gizmoWasClickedThisFrame = false;
|
||||
|
||||
private Vector3 axisDragStartGizmoOrigin;
|
||||
private Vector3 axisDragStartModulePosition;
|
||||
private Vector2 axisDragStartMousePosition;
|
||||
[Header("Axis Drag Settings")]
|
||||
public float axisDragSensitivity = 0.01f;
|
||||
|
||||
public bool WasGizmoClicked()
|
||||
{
|
||||
return gizmoWasClickedThisFrame;
|
||||
}
|
||||
|
||||
public void RefreshGizmoVisibility()
|
||||
{
|
||||
UpdateGizmoVisibility();
|
||||
}
|
||||
|
||||
private LineRenderer xAxisLine;
|
||||
private LineRenderer yAxisLine;
|
||||
private LineRenderer zAxisLine;
|
||||
private GameObject gizmoContainer;
|
||||
|
||||
private Transform xAxisHead;
|
||||
private Transform yAxisHead;
|
||||
private Transform zAxisHead;
|
||||
private static Mesh _coneMesh;
|
||||
|
||||
void Start()
|
||||
{
|
||||
if (mainCamera == null)
|
||||
{
|
||||
mainCamera = Camera.main;
|
||||
if (mainCamera == null)
|
||||
{
|
||||
mainCamera = FindObjectOfType<Camera>();
|
||||
}
|
||||
}
|
||||
|
||||
if (positionController == null)
|
||||
{
|
||||
positionController = GetComponentInParent<ModulePositionController>();
|
||||
if (positionController == null)
|
||||
{
|
||||
positionController = FindObjectOfType<ModulePositionController>();
|
||||
}
|
||||
if (positionController != null && positionController.gizmo == null)
|
||||
{
|
||||
positionController.gizmo = this;
|
||||
}
|
||||
}
|
||||
|
||||
previousColors[0] = xAxisColor;
|
||||
previousColors[1] = yAxisColor;
|
||||
previousColors[2] = zAxisColor;
|
||||
|
||||
FindGizmoLines();
|
||||
}
|
||||
|
||||
void FindGizmoLines()
|
||||
{
|
||||
if (gizmoContainer != null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Transform existingContainer = this.transform.Find("PositionControlGizmo");
|
||||
if (existingContainer != null)
|
||||
{
|
||||
gizmoContainer = existingContainer.gameObject;
|
||||
}
|
||||
else
|
||||
{
|
||||
gizmoContainer = new GameObject("PositionControlGizmo");
|
||||
gizmoContainer.transform.SetParent(this.transform);
|
||||
gizmoContainer.transform.localPosition = Vector3.zero;
|
||||
gizmoContainer.transform.localRotation = Quaternion.identity;
|
||||
gizmoContainer.transform.localScale = Vector3.one;
|
||||
}
|
||||
|
||||
Transform xAxisTransform = gizmoContainer.transform.Find("XAxis");
|
||||
if (xAxisTransform == null)
|
||||
{
|
||||
GameObject xAxisObj = new GameObject("XAxis");
|
||||
xAxisObj.transform.SetParent(gizmoContainer.transform);
|
||||
xAxisObj.transform.localPosition = Vector3.zero;
|
||||
xAxisObj.transform.localRotation = Quaternion.identity;
|
||||
xAxisObj.transform.localScale = Vector3.one;
|
||||
xAxisLine = xAxisObj.AddComponent<LineRenderer>();
|
||||
SetupLineRenderer(xAxisLine, xAxisColor);
|
||||
}
|
||||
else
|
||||
{
|
||||
xAxisLine = xAxisTransform.GetComponent<LineRenderer>();
|
||||
if (xAxisLine == null)
|
||||
{
|
||||
xAxisLine = xAxisTransform.gameObject.AddComponent<LineRenderer>();
|
||||
SetupLineRenderer(xAxisLine, xAxisColor);
|
||||
}
|
||||
else
|
||||
{
|
||||
SetupLineRenderer(xAxisLine, xAxisColor);
|
||||
}
|
||||
}
|
||||
|
||||
Transform yAxisTransform = gizmoContainer.transform.Find("YAxis");
|
||||
if (yAxisTransform == null)
|
||||
{
|
||||
GameObject yAxisObj = new GameObject("YAxis");
|
||||
yAxisObj.transform.SetParent(gizmoContainer.transform);
|
||||
yAxisObj.transform.localPosition = Vector3.zero;
|
||||
yAxisObj.transform.localRotation = Quaternion.identity;
|
||||
yAxisObj.transform.localScale = Vector3.one;
|
||||
yAxisLine = yAxisObj.AddComponent<LineRenderer>();
|
||||
SetupLineRenderer(yAxisLine, yAxisColor);
|
||||
}
|
||||
else
|
||||
{
|
||||
yAxisLine = yAxisTransform.GetComponent<LineRenderer>();
|
||||
if (yAxisLine == null)
|
||||
{
|
||||
yAxisLine = yAxisTransform.gameObject.AddComponent<LineRenderer>();
|
||||
SetupLineRenderer(yAxisLine, yAxisColor);
|
||||
}
|
||||
else
|
||||
{
|
||||
SetupLineRenderer(yAxisLine, yAxisColor);
|
||||
}
|
||||
}
|
||||
|
||||
Transform zAxisTransform = gizmoContainer.transform.Find("ZAxis");
|
||||
if (zAxisTransform == null)
|
||||
{
|
||||
GameObject zAxisObj = new GameObject("ZAxis");
|
||||
zAxisObj.transform.SetParent(gizmoContainer.transform);
|
||||
zAxisObj.transform.localPosition = Vector3.zero;
|
||||
zAxisObj.transform.localRotation = Quaternion.identity;
|
||||
zAxisObj.transform.localScale = Vector3.one;
|
||||
zAxisLine = zAxisObj.AddComponent<LineRenderer>();
|
||||
SetupLineRenderer(zAxisLine, zAxisColor);
|
||||
}
|
||||
else
|
||||
{
|
||||
zAxisLine = zAxisTransform.GetComponent<LineRenderer>();
|
||||
if (zAxisLine == null)
|
||||
{
|
||||
zAxisLine = zAxisTransform.gameObject.AddComponent<LineRenderer>();
|
||||
SetupLineRenderer(zAxisLine, zAxisColor);
|
||||
}
|
||||
else
|
||||
{
|
||||
SetupLineRenderer(zAxisLine, zAxisColor);
|
||||
}
|
||||
}
|
||||
|
||||
xAxisHead = EnsureArrowHead(xAxisLine != null ? xAxisLine.transform : null, "Head");
|
||||
yAxisHead = EnsureArrowHead(yAxisLine != null ? yAxisLine.transform : null, "Head");
|
||||
zAxisHead = EnsureArrowHead(zAxisLine != null ? zAxisLine.transform : null, "Head");
|
||||
|
||||
gizmoContainer.SetActive(false);
|
||||
}
|
||||
|
||||
private Transform EnsureArrowHead(Transform axisTransform, string name)
|
||||
{
|
||||
if (axisTransform == null) return null;
|
||||
|
||||
Transform head = axisTransform.Find(name);
|
||||
if (head == null)
|
||||
{
|
||||
var go = new GameObject(name);
|
||||
go.transform.SetParent(axisTransform, false);
|
||||
head = go.transform;
|
||||
}
|
||||
|
||||
var mf = head.GetComponent<MeshFilter>();
|
||||
if (mf == null) mf = head.gameObject.AddComponent<MeshFilter>();
|
||||
|
||||
var mr = head.GetComponent<MeshRenderer>();
|
||||
if (mr == null) mr = head.gameObject.AddComponent<MeshRenderer>();
|
||||
|
||||
if (_coneMesh == null)
|
||||
{
|
||||
_coneMesh = CreateConeMesh(18);
|
||||
}
|
||||
mf.sharedMesh = _coneMesh;
|
||||
|
||||
if (axisTransform.TryGetComponent<LineRenderer>(out var lr) && lr.material != null)
|
||||
{
|
||||
mr.sharedMaterial = lr.material;
|
||||
}
|
||||
else
|
||||
{
|
||||
var mat = new Material(Shader.Find("Unlit/Color"));
|
||||
mat.color = Color.white;
|
||||
mr.sharedMaterial = mat;
|
||||
}
|
||||
|
||||
mr.shadowCastingMode = UnityEngine.Rendering.ShadowCastingMode.Off;
|
||||
mr.receiveShadows = false;
|
||||
|
||||
return head;
|
||||
}
|
||||
|
||||
private static Mesh CreateConeMesh(int segments)
|
||||
{
|
||||
segments = Mathf.Clamp(segments, 6, 64);
|
||||
|
||||
var mesh = new Mesh();
|
||||
mesh.name = "PositionControlGizmo_Cone";
|
||||
|
||||
int vertCount = segments + 2; // base ring + base center + tip
|
||||
Vector3[] v = new Vector3[vertCount];
|
||||
Vector3[] n = new Vector3[vertCount];
|
||||
Vector2[] uv = new Vector2[vertCount];
|
||||
|
||||
int baseCenter = segments;
|
||||
int tip = segments + 1;
|
||||
|
||||
float radius = 0.5f;
|
||||
for (int i = 0; i < segments; i++)
|
||||
{
|
||||
float a = (i / (float)segments) * Mathf.PI * 2f;
|
||||
float x = Mathf.Cos(a) * radius;
|
||||
float y = Mathf.Sin(a) * radius;
|
||||
v[i] = new Vector3(x, y, 0f);
|
||||
n[i] = (new Vector3(x, y, radius)).normalized; // approximate
|
||||
uv[i] = new Vector2(i / (float)segments, 0f);
|
||||
}
|
||||
|
||||
v[baseCenter] = new Vector3(0f, 0f, 0f);
|
||||
n[baseCenter] = Vector3.back;
|
||||
uv[baseCenter] = new Vector2(0.5f, 0f);
|
||||
|
||||
v[tip] = new Vector3(0f, 0f, 1f);
|
||||
n[tip] = Vector3.forward;
|
||||
uv[tip] = new Vector2(0.5f, 1f);
|
||||
|
||||
int[] tris = new int[segments * 6];
|
||||
int t = 0;
|
||||
for (int i = 0; i < segments; i++)
|
||||
{
|
||||
int next = (i + 1) % segments;
|
||||
|
||||
tris[t++] = i;
|
||||
tris[t++] = next;
|
||||
tris[t++] = tip;
|
||||
|
||||
tris[t++] = next;
|
||||
tris[t++] = i;
|
||||
tris[t++] = baseCenter;
|
||||
}
|
||||
|
||||
mesh.vertices = v;
|
||||
mesh.normals = n;
|
||||
mesh.uv = uv;
|
||||
mesh.triangles = tris;
|
||||
mesh.RecalculateBounds();
|
||||
return mesh;
|
||||
}
|
||||
|
||||
void OnEnable()
|
||||
{
|
||||
UpdateGizmoVisibility();
|
||||
}
|
||||
|
||||
void UpdateGizmoVisibility()
|
||||
{
|
||||
if (gizmoContainer != null)
|
||||
{
|
||||
if (positionController != null)
|
||||
{
|
||||
targetModule = positionController.selectedModule;
|
||||
}
|
||||
|
||||
bool shouldShow = targetModule != null && this.enabled;
|
||||
|
||||
if (shouldShow)
|
||||
{
|
||||
gizmoContainer.SetActive(true);
|
||||
}
|
||||
else
|
||||
{
|
||||
gizmoContainer.SetActive(false);
|
||||
}
|
||||
|
||||
if (!shouldShow)
|
||||
{
|
||||
hoveredAxis = -1;
|
||||
selectedAxis = -1;
|
||||
isDragging = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
FindGizmoLines();
|
||||
}
|
||||
}
|
||||
|
||||
void SetupLineRenderer(LineRenderer lr, Color color)
|
||||
{
|
||||
Shader unlitShader = Shader.Find("Unlit/Color");
|
||||
Material gizmoMaterial;
|
||||
|
||||
if (unlitShader != null)
|
||||
{
|
||||
gizmoMaterial = new Material(unlitShader);
|
||||
gizmoMaterial.color = color;
|
||||
gizmoMaterial.SetInt("_ZTest", (int)UnityEngine.Rendering.CompareFunction.Always);
|
||||
gizmoMaterial.SetInt("_ZWrite", 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
gizmoMaterial = new Material(Shader.Find("Sprites/Default"));
|
||||
gizmoMaterial.color = color;
|
||||
}
|
||||
|
||||
lr.material = gizmoMaterial;
|
||||
lr.startColor = color;
|
||||
lr.endColor = color;
|
||||
lr.startWidth = lineWidth;
|
||||
lr.endWidth = lineWidth;
|
||||
lr.positionCount = 2;
|
||||
lr.useWorldSpace = true;
|
||||
lr.shadowCastingMode = UnityEngine.Rendering.ShadowCastingMode.Off;
|
||||
lr.receiveShadows = false;
|
||||
|
||||
lr.sortingOrder = 1000;
|
||||
}
|
||||
|
||||
void UpdateGizmoVisualization()
|
||||
{
|
||||
if (targetModule == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (gizmoContainer == null)
|
||||
{
|
||||
FindGizmoLines();
|
||||
if (gizmoContainer == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (xAxisLine == null || yAxisLine == null || zAxisLine == null)
|
||||
{
|
||||
FindGizmoLines();
|
||||
if (xAxisLine == null || yAxisLine == null || zAxisLine == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 gizmoOrigin = GetGizmoOrigin();
|
||||
|
||||
Color xColor = GetAxisColor(0);
|
||||
if (previousColors[0] != xColor)
|
||||
{
|
||||
previousColors[0] = xColor;
|
||||
}
|
||||
UpdateAxisLine(xAxisLine, gizmoOrigin, GetAxisDirection(0), arrowLength, xColor);
|
||||
UpdateAxisHead(xAxisHead, gizmoOrigin, GetAxisDirection(0), arrowLength, arrowHeadSize, xColor);
|
||||
|
||||
Color yColor = GetAxisColor(1);
|
||||
if (previousColors[1] != yColor)
|
||||
{
|
||||
previousColors[1] = yColor;
|
||||
}
|
||||
UpdateAxisLine(yAxisLine, gizmoOrigin, GetAxisDirection(1), arrowLength, yColor);
|
||||
UpdateAxisHead(yAxisHead, gizmoOrigin, GetAxisDirection(1), arrowLength, arrowHeadSize, yColor);
|
||||
|
||||
Color zColor = GetAxisColor(2);
|
||||
if (previousColors[2] != zColor)
|
||||
{
|
||||
previousColors[2] = zColor;
|
||||
}
|
||||
UpdateAxisLine(zAxisLine, gizmoOrigin, GetAxisDirection(2), arrowLength, zColor);
|
||||
UpdateAxisHead(zAxisHead, gizmoOrigin, GetAxisDirection(2), arrowLength, arrowHeadSize, zColor);
|
||||
}
|
||||
|
||||
void UpdateAxisHead(Transform head, Vector3 start, Vector3 direction, float length, float headSize, Color color)
|
||||
{
|
||||
if (head == null) return;
|
||||
|
||||
Vector3 dir = direction.normalized;
|
||||
Vector3 end = start + dir * length;
|
||||
|
||||
head.position = end;
|
||||
head.rotation = Quaternion.LookRotation(dir, Vector3.up);
|
||||
head.localScale = new Vector3(headSize, headSize, headSize);
|
||||
|
||||
// Update head material color (shared with line renderer in most cases)
|
||||
var mr = head.GetComponent<MeshRenderer>();
|
||||
if (mr != null && mr.sharedMaterial != null)
|
||||
{
|
||||
if (mr.sharedMaterial.HasProperty("_Color"))
|
||||
mr.sharedMaterial.SetColor("_Color", color);
|
||||
mr.sharedMaterial.color = color;
|
||||
}
|
||||
}
|
||||
|
||||
void UpdateAxisLine(LineRenderer lr, Vector3 start, Vector3 direction, float length, Color color)
|
||||
{
|
||||
if (lr == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Ensure LineRenderer is enabled
|
||||
if (!lr.enabled)
|
||||
{
|
||||
lr.enabled = true;
|
||||
}
|
||||
|
||||
|
||||
Vector3 end = start + direction.normalized * length;
|
||||
lr.SetPosition(0, start);
|
||||
lr.SetPosition(1, end);
|
||||
lr.startColor = color;
|
||||
lr.endColor = color;
|
||||
|
||||
// Also update material color (important for Unlit/Color shader)
|
||||
if (lr.material != null)
|
||||
{
|
||||
lr.material.color = color;
|
||||
// Also set _Color property explicitly (some shaders use this)
|
||||
if (lr.material.HasProperty("_Color"))
|
||||
{
|
||||
lr.material.SetColor("_Color", color);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Update()
|
||||
{
|
||||
if (positionController != null)
|
||||
{
|
||||
targetModule = positionController.selectedModule;
|
||||
}
|
||||
|
||||
UpdateGizmoVisibility();
|
||||
|
||||
if (targetModule != null)
|
||||
{
|
||||
HandleGizmoInteraction();
|
||||
|
||||
UpdateGizmoVisualization();
|
||||
}
|
||||
else
|
||||
{
|
||||
hoveredAxis = -1;
|
||||
selectedAxis = -1;
|
||||
isDragging = false;
|
||||
}
|
||||
}
|
||||
|
||||
void HandleGizmoInteraction()
|
||||
{
|
||||
if (targetModule == null || mainCamera == null)
|
||||
{
|
||||
hoveredAxis = -1;
|
||||
return;
|
||||
}
|
||||
|
||||
if (EventSystem.current != null && EventSystem.current.IsPointerOverGameObject())
|
||||
{
|
||||
hoveredAxis = -1;
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3 gizmoOrigin = GetGizmoOrigin();
|
||||
Vector2 mouseScreenPos = Input.mousePosition;
|
||||
|
||||
float closestDistance = float.MaxValue;
|
||||
int closestAxis = -1;
|
||||
|
||||
for (int axis = 0; axis < 3; axis++)
|
||||
{
|
||||
Vector3 axisDirection = GetAxisDirection(axis);
|
||||
Vector3 axisStart = gizmoOrigin;
|
||||
Vector3 axisEnd = gizmoOrigin + axisDirection * arrowLength;
|
||||
|
||||
Vector3 screenStart3D = mainCamera.WorldToScreenPoint(axisStart);
|
||||
Vector3 screenEnd3D = mainCamera.WorldToScreenPoint(axisEnd);
|
||||
|
||||
if (screenStart3D.z < 0 || screenEnd3D.z < 0)
|
||||
continue;
|
||||
|
||||
Vector2 screenStart = new Vector2(screenStart3D.x, screenStart3D.y);
|
||||
Vector2 screenEnd = new Vector2(screenEnd3D.x, screenEnd3D.y);
|
||||
|
||||
Vector2 closestPointOnLine = ClosestPointOnScreenLine(screenStart, screenEnd, mouseScreenPos);
|
||||
float screenDistance = Vector2.Distance(mouseScreenPos, closestPointOnLine);
|
||||
|
||||
float distanceToStart = Vector2.Distance(mouseScreenPos, screenStart);
|
||||
float minDistance = Mathf.Min(screenDistance, distanceToStart);
|
||||
|
||||
if (minDistance < screenSpaceClickThreshold && minDistance < closestDistance)
|
||||
{
|
||||
closestDistance = minDistance;
|
||||
closestAxis = axis;
|
||||
}
|
||||
}
|
||||
|
||||
int oldHoveredAxis = hoveredAxis;
|
||||
hoveredAxis = closestAxis;
|
||||
|
||||
if (hoveredAxis != oldHoveredAxis)
|
||||
{
|
||||
// no-op (hover state tracking only)
|
||||
}
|
||||
|
||||
gizmoWasClickedThisFrame = false;
|
||||
|
||||
HandleAxisBasedInteraction();
|
||||
}
|
||||
|
||||
void HandleAxisBasedInteraction()
|
||||
{
|
||||
if (EventSystem.current != null && EventSystem.current.IsPointerOverGameObject())
|
||||
{
|
||||
hoveredAxis = -1;
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3 gizmoOrigin = GetGizmoOrigin();
|
||||
Vector2 mouseScreenPos = Input.mousePosition;
|
||||
|
||||
if (Input.GetMouseButtonDown(0))
|
||||
{
|
||||
if (hoveredAxis >= 0)
|
||||
{
|
||||
selectedAxis = hoveredAxis;
|
||||
isDragging = true;
|
||||
axisDragStartGizmoOrigin = gizmoOrigin; // Store starting gizmo origin
|
||||
axisDragStartModulePosition = targetModule.transform.position; // Store module pivot position for IK targets
|
||||
axisDragStartMousePosition = mouseScreenPos; // Store starting mouse position
|
||||
gizmoWasClickedThisFrame = true; // Mark that gizmo was clicked
|
||||
|
||||
if (positionController != null)
|
||||
{
|
||||
positionController.OnGizmoClicked();
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (isDragging && selectedAxis >= 0 && Input.GetMouseButton(0))
|
||||
{
|
||||
ProcessUnityStyleAxisDrag();
|
||||
}
|
||||
else if (Input.GetMouseButtonUp(0))
|
||||
{
|
||||
if (isDragging)
|
||||
{
|
||||
isDragging = false;
|
||||
selectedAxis = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ProcessUnityStyleAxisDrag()
|
||||
{
|
||||
if (targetModule == null || mainCamera == null || positionController == null) return;
|
||||
|
||||
Vector2 currentMousePos = Input.mousePosition;
|
||||
Vector2 mouseDelta = currentMousePos - axisDragStartMousePosition;
|
||||
|
||||
float minMouseMovement = 2f; // Minimum pixels of movement required
|
||||
if (mouseDelta.magnitude < minMouseMovement)
|
||||
{
|
||||
return; // User just clicked without dragging - no movement
|
||||
}
|
||||
|
||||
Vector3 axisDirection = GetAxisDirection(selectedAxis);
|
||||
Vector3 gizmoOrigin = axisDragStartGizmoOrigin; // Use gizmo origin as reference for screen-space projection
|
||||
|
||||
Vector2 screenAxis = (Vector2)mainCamera.WorldToScreenPoint(gizmoOrigin + axisDirection) -
|
||||
(Vector2)mainCamera.WorldToScreenPoint(gizmoOrigin);
|
||||
screenAxis.Normalize();
|
||||
|
||||
float movementAlongAxis = Vector2.Dot(mouseDelta, screenAxis);
|
||||
|
||||
float worldMovement = movementAlongAxis * axisDragSensitivity;
|
||||
|
||||
if (positionController != null && positionController.ikController != null)
|
||||
{
|
||||
worldMovement *= positionController.ikController.dragSensitivity;
|
||||
}
|
||||
|
||||
// IMPORTANT: keep IK target based on module pivot (transform.position), not the visual gizmo origin.
|
||||
Vector3 currentPosition = targetModule.transform.position;
|
||||
Vector3 targetPosition = axisDragStartModulePosition + axisDirection * worldMovement;
|
||||
|
||||
float minMovementThreshold = 0.03f; // Minimum 3cm movement required
|
||||
float distanceToTarget = Vector3.Distance(targetPosition, currentPosition);
|
||||
|
||||
if (distanceToTarget < minMovementThreshold)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3 intendedMovement = targetPosition - currentPosition;
|
||||
float intendedAxisMovement = Vector3.Dot(intendedMovement, axisDirection);
|
||||
float minAxisMovement = 0.02f; // Minimum 2cm movement along the selected axis
|
||||
|
||||
if (Mathf.Abs(intendedAxisMovement) < minAxisMovement)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (positionController.ikController != null)
|
||||
{
|
||||
bool success = positionController.ikController.SolveIK(targetPosition);
|
||||
|
||||
if (success)
|
||||
{
|
||||
Vector3 actualPosition = targetModule.transform.position;
|
||||
Vector3 actualMovement = actualPosition - axisDragStartModulePosition;
|
||||
float actualAxisMovement = Vector3.Dot(actualMovement, axisDirection);
|
||||
float actualMovementDistance = actualMovement.magnitude;
|
||||
|
||||
if (actualMovementDistance >= minMovementThreshold && Mathf.Abs(actualAxisMovement) >= minAxisMovement)
|
||||
{
|
||||
if (Mathf.Sign(actualAxisMovement) == Mathf.Sign(intendedAxisMovement))
|
||||
{
|
||||
axisDragStartModulePosition = actualPosition;
|
||||
axisDragStartGizmoOrigin = GetGizmoOrigin();
|
||||
axisDragStartMousePosition = currentMousePos;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 GetGizmoOrigin()
|
||||
{
|
||||
if (targetModule == null) return Vector3.zero;
|
||||
|
||||
Vector3 origin = targetModule.transform.position;
|
||||
if (!centerOnModuleBounds)
|
||||
return origin + gizmoWorldOffset;
|
||||
|
||||
bool hasBounds = false;
|
||||
Bounds b = default;
|
||||
|
||||
foreach (var r in GetRenderersOwnedByModule(targetModule))
|
||||
{
|
||||
if (r == null) continue;
|
||||
if (!hasBounds)
|
||||
{
|
||||
b = r.bounds;
|
||||
hasBounds = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
b.Encapsulate(r.bounds);
|
||||
}
|
||||
}
|
||||
|
||||
if (!hasBounds)
|
||||
{
|
||||
foreach (var c in GetCollidersOwnedByModule(targetModule))
|
||||
{
|
||||
if (c == null) continue;
|
||||
if (!hasBounds)
|
||||
{
|
||||
b = c.bounds;
|
||||
hasBounds = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
b.Encapsulate(c.bounds);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
origin = hasBounds ? b.center : origin;
|
||||
return origin + gizmoWorldOffset;
|
||||
}
|
||||
|
||||
static System.Collections.Generic.IEnumerable<Renderer> GetRenderersOwnedByModule(ModuleBase rootModule)
|
||||
{
|
||||
if (rootModule == null) yield break;
|
||||
Transform root = rootModule.transform;
|
||||
|
||||
var stack = new System.Collections.Generic.Stack<Transform>();
|
||||
stack.Push(root);
|
||||
|
||||
while (stack.Count > 0)
|
||||
{
|
||||
Transform t = stack.Pop();
|
||||
if (t == null) continue;
|
||||
|
||||
if (t != root)
|
||||
{
|
||||
ModuleBase other = t.GetComponent<ModuleBase>();
|
||||
if (other != null && other != rootModule)
|
||||
continue;
|
||||
}
|
||||
|
||||
foreach (var r in t.GetComponents<Renderer>())
|
||||
if (r != null) yield return r;
|
||||
|
||||
for (int i = 0; i < t.childCount; i++)
|
||||
stack.Push(t.GetChild(i));
|
||||
}
|
||||
}
|
||||
|
||||
static System.Collections.Generic.IEnumerable<Collider> GetCollidersOwnedByModule(ModuleBase rootModule)
|
||||
{
|
||||
if (rootModule == null) yield break;
|
||||
Transform root = rootModule.transform;
|
||||
|
||||
var stack = new System.Collections.Generic.Stack<Transform>();
|
||||
stack.Push(root);
|
||||
|
||||
while (stack.Count > 0)
|
||||
{
|
||||
Transform t = stack.Pop();
|
||||
if (t == null) continue;
|
||||
|
||||
if (t != root)
|
||||
{
|
||||
ModuleBase other = t.GetComponent<ModuleBase>();
|
||||
if (other != null && other != rootModule)
|
||||
continue;
|
||||
}
|
||||
|
||||
foreach (var c in t.GetComponents<Collider>())
|
||||
if (c != null) yield return c;
|
||||
|
||||
for (int i = 0; i < t.childCount; i++)
|
||||
stack.Push(t.GetChild(i));
|
||||
}
|
||||
}
|
||||
|
||||
Vector2 ClosestPointOnScreenLine(Vector2 lineStart, Vector2 lineEnd, Vector2 point)
|
||||
{
|
||||
Vector2 line = lineEnd - lineStart;
|
||||
float lineLength = line.magnitude;
|
||||
|
||||
if (lineLength < 0.001f)
|
||||
return lineStart;
|
||||
|
||||
line.Normalize();
|
||||
Vector2 pointToStart = point - lineStart;
|
||||
float projectionLength = Vector2.Dot(pointToStart, line);
|
||||
projectionLength = Mathf.Clamp(projectionLength, 0f, lineLength);
|
||||
|
||||
return lineStart + line * projectionLength;
|
||||
}
|
||||
|
||||
void AdjustPosition(int axis, int sign)
|
||||
{
|
||||
if (positionController != null)
|
||||
{
|
||||
positionController.AdjustPositionAxis(axis, sign);
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 GetAxisDirection(int axis)
|
||||
{
|
||||
switch (axis)
|
||||
{
|
||||
case 0: return Vector3.right; // X axis (world space - always points right)
|
||||
case 1: return Vector3.up; // Y axis (world space - always points up)
|
||||
case 2: return Vector3.forward; // Z axis (world space - always points forward)
|
||||
default: return Vector3.zero;
|
||||
}
|
||||
}
|
||||
|
||||
Color GetAxisColor(int axis)
|
||||
{
|
||||
if (axis == selectedAxis) return selectedColor;
|
||||
if (axis == hoveredAxis) return hoverColor;
|
||||
|
||||
switch (axis)
|
||||
{
|
||||
case 0: return xAxisColor;
|
||||
case 1: return yAxisColor;
|
||||
case 2: return zAxisColor;
|
||||
default: return Color.white;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 ClosestPointOnLineSegment(Vector3 lineStart, Vector3 lineEnd, Vector3 point)
|
||||
{
|
||||
Vector3 line = lineEnd - lineStart;
|
||||
float lineLength = line.magnitude;
|
||||
line.Normalize();
|
||||
|
||||
Vector3 pointToStart = point - lineStart;
|
||||
float projectionLength = Vector3.Dot(pointToStart, line);
|
||||
projectionLength = Mathf.Clamp(projectionLength, 0f, lineLength);
|
||||
|
||||
return lineStart + line * projectionLength;
|
||||
}
|
||||
|
||||
Vector3 ClosestPointOnRay(Ray ray, Vector3 point)
|
||||
{
|
||||
Vector3 rayToPoint = point - ray.origin;
|
||||
float projectionLength = Vector3.Dot(rayToPoint, ray.direction);
|
||||
projectionLength = Mathf.Max(0f, projectionLength); // Don't go backwards
|
||||
|
||||
return ray.origin + ray.direction * projectionLength;
|
||||
}
|
||||
|
||||
void OnDestroy()
|
||||
{
|
||||
// Don't destroy gizmo container - it's part of the scene hierarchy, not created at runtime
|
||||
// Just clear the reference
|
||||
gizmoContainer = null;
|
||||
xAxisLine = null;
|
||||
yAxisLine = null;
|
||||
zAxisLine = null;
|
||||
}
|
||||
}
|
||||
|
||||
11
Assets/IK/PositionControlGizmo.cs.meta
Normal file
11
Assets/IK/PositionControlGizmo.cs.meta
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 7b174028420e641e388cafa6d51ffcf5
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
Reference in New Issue
Block a user