68 lines
2.3 KiB
C#
68 lines
2.3 KiB
C#
using System;
|
|
using UnityEngine;
|
|
using System.Collections.Generic;
|
|
public class MoveRobotArm : MonoBehaviour
|
|
{
|
|
public Transform target;
|
|
public Transform endPoint;
|
|
public enum eRobotArmRotateAxis
|
|
{
|
|
x, y, z
|
|
}
|
|
[Serializable]
|
|
public struct stRobotArmJoint
|
|
{
|
|
public Transform jointTransform;
|
|
public eRobotArmRotateAxis axisType;
|
|
public float angle;
|
|
public float maxAngle;
|
|
public float minAngle;
|
|
public float baseAngle;
|
|
public float gradient;
|
|
public float delta;
|
|
public void Rotate(float angle)
|
|
{
|
|
switch (axisType)
|
|
{
|
|
case eRobotArmRotateAxis.x:
|
|
jointTransform.eulerAngles = new Vector3(angle, jointTransform.eulerAngles.y, jointTransform.eulerAngles.z);
|
|
break;
|
|
case eRobotArmRotateAxis.y:
|
|
jointTransform.eulerAngles = new Vector3(jointTransform.eulerAngles.x, angle, jointTransform.eulerAngles.z);
|
|
break;
|
|
case eRobotArmRotateAxis.z:
|
|
jointTransform.eulerAngles = new Vector3(jointTransform.eulerAngles.x, jointTransform.eulerAngles.y, angle);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
public List<stRobotArmJoint> joints = new List<stRobotArmJoint>();
|
|
// Start is called once before the first execution of Update after the MonoBehaviour is created
|
|
void Start()
|
|
{
|
|
for (int i = 0; i < joints.Count; i++)
|
|
{
|
|
stRobotArmJoint joint = joints[i];
|
|
joint.gradient = 1.0f;
|
|
}
|
|
}
|
|
|
|
// Update is called once per frame
|
|
void Update()
|
|
{
|
|
if (target == null)
|
|
return;
|
|
for (int i = 0; i < joints.Count; i++)
|
|
{
|
|
stRobotArmJoint joint = joints[i];
|
|
float distance = Vector3.Distance(target.position, endPoint.position);
|
|
float angle = joint.angle;
|
|
joint.Rotate(angle + joint.gradient);
|
|
float checkDistanceP = Vector3.Distance(target.position, endPoint.position);
|
|
joint.Rotate(angle - joint.gradient);
|
|
float checkDistanceN = Vector3.Distance(target.position, endPoint.position);
|
|
//if (checkDistanceP > )
|
|
}
|
|
}
|
|
}
|