void CalculateServoGroupOffset(ref IKAxis IKAxis)
{
// Debug.Log("servo axis count " + IKAxis.servoGroup.Count + " ");
var servoGroup = IKAxis.servoGroup;
for (int i = 0; i < servoGroup.Count; i++)
{
//set the wrist dist
if (i == 0)
{
servoGroup[i].limbLength = Vector3.Distance(servoGroup[i].transform.position, limbController.contactPointOffset);
var limbOffset = servoGroup[i].servoBase.InverseTransformPoint(limbEnd.position);
var tempAngle = (float)(Math.Atan2(limbOffset.z, limbOffset.y));
tempAngle = (float)(tempAngle * 180 / Math.PI);
servoGroup[i].targetOffset = tempAngle;
}
else
{
servoGroup[i].limbLength = Vector3.Distance(servoGroup[i].transform.position, servoGroup[i - 1].transform.position);
var limbOffset = servoGroup[i].servoBase.InverseTransformPoint(servoGroup[i - 1].transform.position);
var tempAngle = (float)(Math.Atan2(limbOffset.z, limbOffset.y));
tempAngle = (float)(tempAngle * 180 / Math.PI);
servoGroup[i].targetOffset = tempAngle;
}
}
if (servoGroup.Count > 0)
{
if (servoGroup[0].targetAngle != Vector3.zero)
{
Debug.Log("0 joint has set angle");
IKAxis.fixedAngleServo = servoGroup[0];
if (servoGroup.Count > 1)
{
IKAxis.servo0 = servoGroup[1];
}
if (servoGroup.Count > 2)
{
IKAxis.servo1 = servoGroup[2];
}
}
else
{
if (servoGroup.Count > 0)
{
IKAxis.servo0 = servoGroup[0];
// Debug.Log("set servo0");
}
if (servoGroup.Count > 1)
{
IKAxis.servo1 = servoGroup[1];
}
}
}
}
public void StoreGroupedServos()
{
Debug.Log("store grouped servos");
IKAxisX = new IKAxis();
IKAxisX.servoGroup = new List <RoboticServoIK>();
IKAxisY = new IKAxis();
IKAxisY.servoGroup = new List <RoboticServoIK>();
IKAxisZ = new IKAxis();
IKAxisZ.servoGroup = new List <RoboticServoIK>();
foreach (var servo in servosIK)
{
if (!servo.disabled)
{
switch (servo.limbAxis)
{
case LimbController.LimbAxis.X:
IKAxisX.servoGroup.Add(servo);
break;
case LimbController.LimbAxis.Y:
IKAxisY.servoGroup.Add(servo);
break;
case LimbController.LimbAxis.Z:
IKAxisZ.servoGroup.Add(servo);
break;
}
}
else
{
Debug.Log("Did not add to servo list " + servo.name);
}
}
IKAxisX.servoGroup.Reverse();
IKAxisY.servoGroup.Reverse();
IKAxisZ.servoGroup.Reverse();
CalculateServoGroupOffset(ref IKAxisX);
CalculateServoGroupOffset(ref IKAxisY);
CalculateServoGroupOffset(ref IKAxisZ);
foreach (var item in IKAxisX.servoGroup)
{
// Debug.Log(item.name + "dddddddd");
}
}
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);
// }
// }
// // }
}
