public void ActivateIK()
{
Debug.Log("activate Ik");
robotStatus = RobotStatus.Idle;
baseTargets = new GameObject("Base Targets").transform;
baseTargets.SetParent(GameObject.Find("Vessel Offset").transform);
baseTargets.localEulerAngles = Vector3.zero;
baseTargets.SetParent(null);
baseTargets.localPosition = Vector3.zero;
directionTarget = new GameObject("Direction Target").transform;
directionTarget.SetParent(GameObject.Find("Gimbal").transform);
directionTarget.localEulerAngles = GameObject.Find("Mirror Vessel COM").transform.localEulerAngles;
targetBaseHeight = directionTarget.position;
foreach (var limb in limbs)
{
var baseTarget = Instantiate(GameObject.Find("Base Target")).transform;
baseTarget.SetParent(baseTargets);
limb.SetBaseTarget(baseTarget);
limb.ActivateIK();
}
baseTargets.SetParent(directionTarget);
IKactivaed.Invoke();
DebugVector.DrawVector(directionTarget, DebugVector.Direction.z, 3, .1f, Color.red, Color.white, Color.green);
DebugVector.DrawVector(memoryBridge.vesselControl.mirrorVessel.vesselOffset, DebugVector.Direction.z, 3, .1f, Color.red, Color.white, Color.blue);
steeringPoint = new GameObject("steering point");
DebugVector.DrawVector(steeringPoint.transform, DebugVector.Direction.all, 2, .1f, Color.red, Color.white, Color.blue);
// Debug.LogError("");
}
public void SetHomePoint(Vector3 position)
{
Debug.Log("Home point updated");
homePoint = new GameObject("Home Point").transform;
homePoint.position = position;
DebugVector.DrawVector(homePoint, Color.red, Color.green, Color.blue, Vector3.one);
}
private void Start()
{
for (int i = 0; i < flightPoints.Count; i++)
{
DebugVector.DrawVector(flightPoints[i].transform, Color.red, Color.green, Color.blue, Vector3.one);
//if (i != 0)
//{
// flightPoints[i].LookAt(flightPoints[i -1], Vector3.up);
//}
//else
//{
// flightPoints[i].LookAt(flightPoints[flightPoints.Count - 1], Vector3.up);
//}
//flightPoints[i].localEulerAngles += new Vector3(0, 180, 0);
}
}
void CalculateIK(IKAxis IKAxis)
{
var servoGroup = IKAxis.servoGroup;
if (IKAxis.fixedAngleServo)
{
var targetOffset = IKAxis.fixedAngleServo.servoBase.InverseTransformPoint(IKAxis.fixedAngleServo.servoBase.transform.position + new Vector3(0, -100, 0));
var angle = Math.Atan2(targetOffset.z, targetOffset.y);
angle *= (180 / Math.PI);
IKAxis.fixedAngleServo.SetServo((float)angle);
//IKAxis.fixedAngleServo.LookDown();
}
if (IKAxis.servo1)
{
var targetOffset = IKAxis.servo1.servoBase.InverseTransformPoint(IKtargetTransform.position);
var angle0 = Math.Atan2(targetOffset.z, targetOffset.y);
angle0 *= (180 / Math.PI);
if (firstFrame)
{
//var children = GetComponentsInChildren<Transform>();
//foreach (var child in children)
//{
// if (child.name == "Foot(Clone)")
// {
// trueLimbEnd = child;
// DebugVector.DrawVector(trueLimbEnd, DebugVector.Direction.all, 5f, .1f, Color.red, Color.white, Color.blue);
// }
//}
// Debug.Log("servo 1 offset " + IKAxis.servo1.targetOffset);
firstFrame = false;
var axis = IKAxisZ;
if (axis.servo0)
{
DebugVector.DrawVector(axis.servo0.transform, DebugVector.Direction.all, .5f, .1f, Color.red, Color.white, Color.blue);
}
if (axis.servo1)
{
DebugVector.DrawVector(IKAxis.servo1.transform, DebugVector.Direction.all, .5f, .1f, Color.red, Color.white, Color.blue);
}
//DebugVector.DrawVector(IKAxis.servo1.transform, DebugVector.Direction.all, .5f, .1f, Color.red, Color.white, Color.blue);
}
servo1_0Dist = Vector3.Distance(IKAxis.servo1.transform.position, IKAxis.servo0.transform.position);
var localPos = IKAxis.servo0.transform.InverseTransformPoint(trueLimbEnd.position);
localPos.x = 0;
var globalPos = IKAxis.servo0.transform.TransformPoint(localPos);
servo0_GroundPointDist = Vector3.Distance(IKAxis.servo0.transform.position, globalPos);
servo1_GroundPointDist = Vector3.Distance(IKAxis.servo1.transform.position, IKtargetTransform.position);
float angle1 = LawOfCosines(servo1_0Dist, servo0_GroundPointDist, servo1_GroundPointDist);
//var angle1 = LawOfCosines(servo1_0Dist, )
if (IKAxis.servo1.invert)
{
angle1 -= (float)angle0;
}
else
{
angle1 += (float)angle0;
}
// var angle = (float)(angle1 + angle0);// - IKAxis.servo1.targetOffset);
if (!float.IsNaN(angle1))
{
IKAxis.servo1.SetServo(angle1);
}
// if (limbController.name == "ik Test")
// {
//Debug.Log(Time.frameCount);
//// Debug.Log(" limblength " + IKAxis.servo0.limbLength);
//Debug.Log(" servo1_0Dist " + servo1_0Dist);
//Debug.Log(" servo0_groundPoint " + servo0_GroundPointDist);
//Debug.Log(" servo1_ground point " + servo1_GroundPointDist);
//Debug.Log("Angle 1 " + angle0);
//Debug.Log("Angle 2 " + angle1);
}
if (IKAxis.servo0)
{
if (IKAxis.fixedAngleServo)
{
// Debug.Log("run servo 0 fixed angle");
// var yDif = IKAxis.servo0.groundPoint.position.y - IKtargetTransform.position.y;
// Debug.Log(yDif);
// var limbDist = Vector3.Distance(IKAxis.fixedAngleServo.transform.position, IKAxis.fixedAngleServo.groundPoint.position);
var targetPos = IKtargetTransform.position; // + new Vector3(0, limbDist, 0);
// if (testTarget)
// testTarget.position = targetPos;
var targetOffset = IKAxis.servo0.servoBase.InverseTransformPoint(targetPos);
var angle = Math.Atan2(targetOffset.z, targetOffset.y);
angle *= (180 / Math.PI);
// angle += IKAxis.fixedAngleServo.setAngle;
// angle = -IKAxis.fixedAngleServo.setAngle;
// IKAxis.servo0.SetServo((float)angle);// + IKAxis.fixedAngleServo.setAngle);
}
else
{
var footOffset = IKAxis.servo0.transform.InverseTransformPoint(trueLimbEnd.position);
xOffset = footOffset.z;
Vector3 targetOffset;
//if (IKAxis.servo0 == servoBase && Math.Abs(footOffset.z) > .1f)
// targetOffset = IKAxis.servo0.servoBase.InverseTransformPoint(IKtargetTransform.position - (IKAxis.servo0.transform.forward * footOffset.z));
//else
//{
targetOffset = IKAxis.servo0.servoBase.InverseTransformPoint(IKtargetTransform.position);
// }
var angle = Math.Atan2(targetOffset.z, targetOffset.y);
angle *= (180 / Math.PI);
// Debug.Log(" servo 0 " + IKAxis.servo0.name);
// angle -= IKAxis.servo0.targetOffset;
IKAxis.servo0.SetServo((float)angle);
}
}
}
void CalculateIK(IKAxis IKAxis)
{
var servoGroup = IKAxis.servoGroup;
if (IKAxis.fixedAngleServo)
{
var targetOffset = IKAxis.fixedAngleServo.servoBase.InverseTransformPoint(IKAxis.fixedAngleServo.servoBase.transform.position + new Vector3(0, -100, 0));
var angle = Math.Atan2(targetOffset.z, targetOffset.y);
angle *= (180 / Math.PI);
IKAxis.fixedAngleServo.SetServo((float)angle);
//IKAxis.fixedAngleServo.LookDown();
}
if (IKAxis.servo0)
{
if (IKAxis.fixedAngleServo)
{
Debug.Log("run servo 0 fixed angle");
var yDif = IKAxis.servo0.groundPoint.position.y - targetTransform.position.y;
Debug.Log(yDif);
var limbDist = Vector3.Distance(IKAxis.fixedAngleServo.transform.position, IKAxis.fixedAngleServo.groundPoint.position);
var targetPos = targetTransform.position;// + new Vector3(0, limbDist, 0);
if (testTarget)
{
testTarget.position = targetPos;
}
var targetOffset = IKAxis.servo0.servoBase.InverseTransformPoint(targetPos);
var angle = Math.Atan2(targetOffset.z, targetOffset.y);
angle *= (180 / Math.PI);
// angle += IKAxis.fixedAngleServo.setAngle;
// angle = -IKAxis.fixedAngleServo.setAngle;
// IKAxis.servo0.SetServo((float)angle);// + IKAxis.fixedAngleServo.setAngle);
}
else
{
// Debug.Log("run servo 0");
var targetOffset = IKAxis.servo0.servoBase.InverseTransformPoint(targetTransform.position);
var angle = Math.Atan2(targetOffset.z, targetOffset.y);
angle *= (180 / Math.PI);
// angle -= IKAxis.servo0.targetOffset;
IKAxis.servo0.SetServo((float)angle);
}
}
if (IKAxis.servo1)
{
// Debug.Log("run servo 2");
var targetOffset = IKAxis.servo1.servoBase.InverseTransformPoint(targetTransform.position);
var angle0 = Math.Atan2(targetOffset.z, targetOffset.y);
angle0 *= (180 / Math.PI);
// float angle1 = LawOfCosines(IKAxis.servo1.limbLength, IKAxis.servo0.limbLength, Vector3.Distance(IKAxis.servo1.transform.position, targetTransform.position));
// if (limbDistCheckCount == 0)
// var dist = Vector3.Distance(IKAxis.servo0.groundPoint.position, targetTransform.position);
// if(dist > .5f)
// {
// var 8 = IKAxis.servo1.limbLength
if (firstFrame)
{
var children = GetComponentsInChildren <Transform>();
foreach (var child in children)
{
if (child.name == "Foot(Clone)")
{
limbEndPoint = child;
}
}
// Debug.Log("servo 1 offset " + IKAxis.servo1.targetOffset);
firstFrame = false;
var axis = IKAxisZ;
if (axis.servo0)
{
DebugVector.DrawVector(axis.servo0.transform, DebugVector.Direction.all, .5f, .1f, Color.red, Color.white, Color.blue);
}
if (axis.servo1)
{
DebugVector.DrawVector(IKAxis.servo1.transform, DebugVector.Direction.all, .5f, .1f, Color.red, Color.white, Color.blue);
}
//DebugVector.DrawVector(IKAxis.servo1.transform, DebugVector.Direction.all, .5f, .1f, Color.red, Color.white, Color.blue);
}
servo1_0Dist = Vector3.Distance(IKAxis.servo1.transform.position, IKAxis.servo0.transform.position);
servo0_GroundPointDist = Vector3.Distance(IKAxis.servo0.transform.position, limbEndPoint.position);
servo1_GroundPointDist = Vector3.Distance(IKAxis.servo1.transform.position, targetTransform.position);
// limbDistCheckCount = 10;
// }
// limbDistCheckCount--;
float angle1 = LawOfCosines(servo1_0Dist, servo0_GroundPointDist, servo1_GroundPointDist);
//float angle1 = LawOfCosines(IKAxis.servo1.limbLength, Vector3.Distance(IKAxis.servo0.transform.position,targetTransform.position), Vector3.Distance(IKAxis.servo1.transform.position, targetTransform.position));
if (IKAxis.servo1.invert)
{
angle1 -= (float)angle0;
}
else
{
angle1 += (float)angle0;
}
// var angle = (float)(angle1 + angle0);// - IKAxis.servo1.targetOffset);
if (!float.IsNaN(angle1))
{
IKAxis.servo1.SetServo(angle1);
}
// if (limbController.name == "ik Test")
// {
//Debug.Log(Time.frameCount);
//// Debug.Log(" limblength " + IKAxis.servo0.limbLength);
//Debug.Log(" servo1_0Dist " + servo1_0Dist);
//Debug.Log(" servo0_groundPoint " + servo0_GroundPointDist);
//Debug.Log(" servo1_ground point " + servo1_GroundPointDist);
//Debug.Log("Angle 1 " + angle0);
//Debug.Log("Angle 2 " + angle1);
// }//
}
// }
// // for (int i = 0; i < servoGroup.Count; i++)
//// {
// var servo = servoGroup[i];
// if (i == 0)
// {
// if (servo.targetAngle != Vector3.down)
// {
// var targetOffset = servoGroup[0].servoBase.InverseTransformPoint(targetTransform.position);
// var angle = Math.Atan2(targetOffset.z, targetOffset.y);
// angle *= (180 / Math.PI);
// servoGroup[0].SetServo((float)angle);
// }
// else
// {
// var targetOffset = servo.servoBase.InverseTransformPoint(Vector3.down);
// var angle = Math.Atan2(targetOffset.z, targetOffset.y);
// angle *= (180 / Math.PI);
// servoGroup[0].SetServo(-(float)angle);
// }
// }
// if (i == 1 || i == 2)
// {
// Debug.Log("run servo 2");
// var targetOffset = servoGroup[i].servoBase.InverseTransformPoint(targetTransform.position);
// var angle0 = Math.Atan2(targetOffset.z, targetOffset.y);
// angle0 *= (180 / Math.PI);
// float angle1 = LawOfCosines(servoGroup[i].limbLength, servoGroup[i - 1].limbLength, Vector3.Distance(servoGroup[i].transform.position, targetTransform.position));
// //float angle1 = LawOfCosines(Vector3.Distance(servoGroup[i].transform.position, servoGroup[0].transform.position), servoGroup[0].limbLength, Vector3.Distance(servoGroup[i].transform.position, targetTransform.position));
// if (servo.invert)
// {
// angle1 -= (float)angle0;
// }
// else
// {
// angle1 += (float)angle0;
// }
// if (!float.IsNaN((float)angle1))
// {
// servoGroup[i].SetServo((float)angle1);
// }
// else
// {
// // servoGroup[i].SetServo((float)angle0);
// }
// }
// // }
}
// Use this for initialization
void Start()
{
target = transform.Find("Target");
DebugVector.DrawVector(target, Color.red, Color.green, Color.blue, Vector3.one);
}
