public void MoveC(IK robot, Transform finalPoint, Transform middlePoint)
{
List <Transform> list = new List <Transform>();
list.Add(robot.GetE());
list.Add(middlePoint);
list.Add(finalPoint);
Vector3[] positions = spline.PrepareSpline(list);
List <Vector3> points = spline.GetTrayectory(positions);
Transform[] trayectory = new Transform[points.Count - 1]; // skip first one, Efector
for (int i = 0; i < trayectory.Length - 1; i++) // Last one is target, skip
{
Transform transf = new GameObject().transform;
transf.position = points[i + 1];
trayectory[i] = transf;
}
trayectory[trayectory.Length - 1] = finalPoint;
robot.CCDAlg(trayectory, false);
//
if (gameController.GetOnlineMode())
{
controller.RunCommandOnline("move" + " " + finalPoint.GetComponent <TargetModel>().GetName() + " " + middlePoint.GetComponent <TargetModel>().GetName());
}
}
public override void OnInspectorGUI()
{
ik = target as IK;
var bone = EditorGUILayout.ObjectField("Start Bone", ik.StartBone, typeof(Bone), true) as Bone;
if (bone != ik.StartBone)
{
ik.StartBone = bone;
InitBones();
}
bone = EditorGUILayout.ObjectField("End Bone", ik.EndBone, typeof(Bone), true) as Bone;
if (bone != ik.EndBone)
{
ik.EndBone = bone;
InitBones();
}
EditorGUILayout.Space();
showBones = EditorGUILayout.Foldout(showBones, "Bones");
if (showBones)
{
GUIStyle style = new GUIStyle();
style.margin.left = 20;
EditorGUILayout.BeginVertical(style);
for (var i = 0; i < ik.Bones.Length; i++)
{
EditorGUILayout.BeginHorizontal();
EditorGUILayout.ObjectField(ik.Bones[i], typeof(Bone), true);
EditorGUILayout.Vector3Field("", ik.Bones[i].transform.localRotation.eulerAngles);
EditorGUILayout.EndHorizontal();
}
EditorGUILayout.EndVertical();
}
}
public static void SaveItemIK(string IKSetting, string itemName,
Vector3 objPosition, Vector3 objEulerAngles, Vector3 objScale,
Vector3 leftHandPosition, Vector3 leftHandEulerAngles,
Vector3 rightHandPosition, Vector3 rightHandEulerAngles)
{
PlayerClass pc = instance.iks.list.Find(x => x.id == IKSetting);
if (pc == null)
{
pc = new PlayerClass();
pc.id = IKSetting;
instance.iks.list.Add(pc);
}
IK ik = pc.list.Find(x => x.ItemName == itemName);
if (ik == null)
{
ik = new IK();
ik.ItemName = itemName;
pc.list.Add(ik);
}
ik.objLocalPosition = objPosition;
ik.objLocalEulerAngles = objEulerAngles;
ik.objLocalScale = objScale;
ik.leftHandEulerAngles = leftHandEulerAngles;
ik.leftHandPosition = leftHandPosition;
ik.rightHandEulerAngles = rightHandEulerAngles;
ik.rightHandPosition = rightHandPosition;
instance.SaveToFile = true;
}
public void MoveL(IK robot, Transform target)
{
Vector3 a = robot.GetE().position;
Vector3 b = target.position;
float resolution = 0.1f; // Line quality, small=high
int loops = Mathf.FloorToInt(1f / resolution);
// Linear trayectory
Transform[] trayectory = new Transform[loops];
for (int i = 1; i < loops; i++) // Last one is target, skip
{
Transform transf = new GameObject().transform;
float t = i * resolution;
//transf.position = Vector3.Lerp(robot.E.position, target.position, t);
transf.position = Vector3.Lerp(a, b, t);
trayectory[i - 1] = transf;
}
trayectory[loops - 1] = target;
robot.CCDAlg(trayectory, false);
//
if (gameController.GetOnlineMode())
{
controller.RunCommandOnline("movel" + " " + target.GetComponent <TargetModel>().GetName());
}
}
/**
* Activa el Scorbot especificado por su índice.
* @param index Índice del Scorbot
* @return void
*/
public void SetScorbot(int index)
{
// Hide all Scorbots
foreach (IK scorbot in scorbots)
{
scorbot.gameObject.SetActive(false);
}
// Activate selected Scorbot
switch (index)
{
case ScorbotERIX.INDEX:
scorbots[ScorbotERIX.INDEX].gameObject.SetActive(true);
robot = scorbots[ScorbotERIX.INDEX];
GameController.indexRobot = ScorbotERIX.INDEX;
break;
case ScorbotERVPlus.INDEX:
scorbots[ScorbotERVPlus.INDEX].gameObject.SetActive(true);
robot = scorbots[ScorbotERVPlus.INDEX];
GameController.indexRobot = ScorbotERVPlus.INDEX;
break;
case ScorbotERIXV2.INDEX:
scorbots[ScorbotERIXV2.INDEX].gameObject.SetActive(true);
robot = scorbots[ScorbotERIXV2.INDEX];
GameController.indexRobot = ScorbotERIXV2.INDEX;
break;
}
ScorbotDel(robot);
}
private void ChangeIK()
{
for (int i = 0; i < 4; ++i)
{
if (i != _index)
{
_ikEnabled[i] = false;
_ikTextList[i].color = _defaultColor;
_ikList[i].ikActive = false;
}
}
bool now = !_ikEnabled[_index];
_ikEnabled[_index] = now;
_ikList[_index].ikActive = now;
if (now)
{
_ikNow = _ikList[_index];
_ikTextList[_index].color = Color.red;
}
else
{
_ikNow = null;
_ikTextList[_index].color = _defaultColor;
_ikObj.SetParent(null);
_ikObj.gameObject.SetActive(false);
_index = -1;
}
}
public void Transform()
{
if (!IsEnable || Target == null || IKLinksBones.Length == 0)
return;
InitializeAngle();
CalcBonePosition(IKLinksBones.Length - 1);
_ikPosition = IK.GetTransformedBonePosition();
Target.UpdateLocalMatrix();
_targetPosition = Target.GetTransformedBonePosition();
if ((_ikPosition - _targetPosition).LengthSquared < 1E-08f)
return;
int num = LoopCount / 2;
for (int i = 0; i < LoopCount; i++)
{
for (int j = 0; j < IKLinksBones.Length; j++)
{
if (!_fixAxis[j])
{
IKProc_Link(j, i < num);
}
}
if ((_ikPosition - _targetPosition).LengthSquared < 1E-08f)
break;
}
}
public override void OnInspectorGUI()
{
DrawDefaultInspector();
IK myScript = (IK)target;
EditorGUILayout.Space();
EditorGUILayout.BeginHorizontal();
EditorGUILayout.Space();
if (GUILayout.Button(myScript.invert ? "Inverted" : "Not Inveted"))
{
myScript.invert = !myScript.invert;
}
EditorGUILayout.Space();
EditorGUILayout.EndHorizontal();
EditorGUILayout.Space();
EditorGUILayout.BeginHorizontal();
if (GUILayout.Button(myScript.toggleIK?"IK ON": "IK OFF"))
{
myScript.toggleIK = !myScript.toggleIK;
}
if (GUILayout.Button("Update Bones"))
{
myScript.UpdateBones();
}
EditorGUILayout.EndHorizontal();
}
/**
* Mueve el Scorbot a una posición ya definida.
* @param robot Scorbot
* @param target Posición (objeto)
* @return void
*/
public void Move(IK robot, Transform target)
{
// Target with valid data
if (target.GetComponent <TargetModel>().GetValid())
{
// Move Scorbot to target. It uses "speed"
robot.Move(target);
}
else // Target with invalid data
{
stateMessageControl.WriteMessage("Error. MOVE Unreachable position \"" + target.GetComponent <TargetModel>().GetName() + "\"", false);
return;
}
stateMessageControl.WriteMessage("Done. MOVE \"" + target.GetComponent <TargetModel>().GetName() + "\"", true);
// Online mode
if (gameController.GetOnlineMode())
{
bool done = controller.RunCommandUIOnline("move", target.GetComponent <TargetModel>().GetName());
if (done)
{
string aux = "";
if (!target.GetComponent <TargetModel>().GetSync())
{
aux = ". NO SYNC";
}
stateMessageControl.WriteMessage("Done. Online MOVE \"" + target.GetComponent <TargetModel>().GetName() + "\"" + aux, done);
}
else
{
stateMessageControl.WriteMessage("Error. Online MOVE \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
}
}
}
//Transitions
public override IEnumerator EnterState(BaseState prevState)
{
if (currentNodes[0].transform.root.tag == "Enemy/Cyclops")
{
currentNodes[0].transform.root.GetComponent <CyclopsData>().beingClimbed = true;
}
rb.velocity = Vector3.zero;
rb.useGravity = false;
IK.HeadWeight = 0;
elapsedTime = 0;
while (elapsedTime < 0.5f)
{
if (!moving)
{
IK.SetIKPositions(currentNodes[0].rightHand, currentNodes[1].leftHand, currentNodes[0].rightFoot, currentNodes[1].leftFoot);
}
rb.transform.position = Vector3.Lerp(rb.transform.position, (currentNodes[1].PlayerPosition + currentNodes[0].PlayerPosition) / 2, elapsedTime * 2);
Quaternion desiredRotation = currentNodes[0].transform.rotation;
if (freehang)
{
desiredRotation = Quaternion.Euler(0, desiredRotation.eulerAngles.y, desiredRotation.eulerAngles.z);
}
rb.transform.rotation = Quaternion.Lerp(rb.transform.rotation, desiredRotation, elapsedTime * 2);
IK.GlobalWeight = Mathf.Lerp(IK.RightHand.weight, 1, elapsedTime * 2);
IK.RightFoot.weight = Mathf.Lerp(IK.RightFoot.weight, freehang ? 0 : 1, elapsedTime * 2);
IK.LeftFoot.weight = IK.RightFoot.weight;
elapsedTime += Time.deltaTime;
yield return(null);
}
yield return(base.EnterState(prevState));
}
public void InitBones()
{
ik = target as IK;
if (ik.StartBone && ik.EndBone)
{
ik.Bones = SearchBones(ik.EndBone.transform, ik.StartBone).ToArray();
}
}
void Start()
{
// Find the ik component
ik = GetComponent <IK>();
// Store the default rest position of the leg
defaultDirection = mechSpider.transform.TransformDirection(position + offset - mechSpider.transform.position);
}
// Online
public void SyncSimulationToScorbot(IK robot, Transform target)
{
// Teach to Scorbot
Vector3 pos = target.GetComponent <TargetModel>().GetPositionInScorbot();
float p = target.GetComponent <TargetModel>().GetPitch();
float r = target.GetComponent <TargetModel>().GetRoll();
Teach(robot, target, pos, p, r);
}
public IKList(BinaryReader bin)
{
ik_data_count = bin.ReadUInt16();
ik_data = new IK[ik_data_count];
for (int i = 0; i < ik_data_count; i++)
{
ik_data[i] = new IK(bin);
}
}
private void Start()
{
this.ik = base.GetComponent <IK>();
this.stepProgress = 1f;
this.hit = default(RaycastHit);
IKSolver.Point[] points = this.ik.GetIKSolver().GetPoints();
this.position = points[points.Length - 1].transform.position;
this.hit.point = this.position;
this.defaultPosition = this.mechSpider.transform.InverseTransformPoint(this.position + this.offset);
}
// Use this for initialization
void Start()
{
Load_Command();
TargetControl = GameC.GetComponent <TargetControl>();
CommandControl = GameC.GetComponent <CommandControl>();
Robot = GameC.GetComponent <GameController>().robot;
Online = false;
GameController.OnlineDel += Online_Offline;
}
/**
* Mueve el Scorbot a una posición ya definida. El movimiento es una línea recta.
* @param robot Scorbot
* @param target Posición (objeto)
* @return void
*/
public void MoveL(IK robot, Transform target)
{
// If target with invalid data
if (!target.GetComponent <TargetModel>().GetValid())
{
stateMessageControl.WriteMessage("Error. MOVEL Unreachable position \"" + target.GetComponent <TargetModel>().GetName() + "\"", false);
return;
}
// Scorbot end effector
Vector3 a = robot.GetE().position;
// Position coordinates
Vector3 b = target.position;
float resolution = 0.1f; // Line quality, small=high quality
int loops = Mathf.FloorToInt(1f / resolution);
// Linear trajectory
Transform[] trajectory = new Transform[loops];
for (int i = 1; i < loops; i++) // Last one is target, skip
{
// New object
Transform transf = new GameObject().transform;
float t = i * resolution;
// Modify coordinates. Interpolation from a to b
transf.position = Vector3.Lerp(a, b, t);
// Add to trajectory
trajectory[i - 1] = transf;
}
// Add target to trajectory
trajectory[loops - 1] = target;
// Move Scorbot following trajectory. It uses "speedl"
robot.CCDAlg(trajectory, false);
stateMessageControl.WriteMessage("Done. MOVEL \"" + target.GetComponent <TargetModel>().GetName() + "\"", true);
// Online mode
if (gameController.GetOnlineMode())
{
bool done = controller.RunCommandUIOnline("movel", target.GetComponent <TargetModel>().GetName());
if (done)
{
string aux = "";
if (!target.GetComponent <TargetModel>().GetSync())
{
aux = ". NO SYNC";
}
stateMessageControl.WriteMessage("Done. Online MOVEL \"" + target.GetComponent <TargetModel>().GetName() + "\"" + aux, done);
}
else
{
stateMessageControl.WriteMessage("Error. Online MOVEL \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
}
}
}
public override void UpdateIK()
{
if (nextNode)
{
float duration = (Player.transform.position - nextNode.leftHand.position).magnitude * MovementSpeed * Time.deltaTime;
IK.MoveRoot(nextNode.transform, duration, Offset);
if (nextMove == RIGHT)
{
float handDuration = (currentRight.rightHand.position - nextNode.rightHand.position).magnitude * MovementSpeed * Time.deltaTime;
float footDuration = (currentRight.rightFoot.position - nextNode.rightFoot.position).magnitude * MovementSpeed * Time.deltaTime;
bool done = true;
if (!(IK.MoveRightHand(nextNode.rightHand.transform, handDuration)))
{
done = false;
}
if (!(IK.MoveRightFoot(nextNode.rightFoot.transform, footDuration)))
{
done = false;
}
if (done)
{
currentRight = nextNode;
currentNode = nextNode;
lastMove = RIGHT;
lastIndex = nodeIndex;
moving = false;
}
}
else if (nextMove == LEFT)
{
float handDuration = (currentLeft.leftHand.position - nextNode.leftHand.position).magnitude * MovementSpeed * Time.deltaTime;
float footDuration = (currentLeft.leftFoot.position - nextNode.leftFoot.position).magnitude * MovementSpeed * Time.deltaTime;
bool done = true;
if (!(IK.MoveLeftHand(nextNode.leftHand.transform, handDuration)))
{
done = false;
}
if (!(IK.MoveLeftFoot(nextNode.leftFoot.transform, footDuration)))
{
done = false;
}
if (done)
{
currentLeft = nextNode;
currentNode = nextNode;
lastMove = LEFT;
lastIndex = nodeIndex;
moving = false;
}
}
}
}
// Use this for initialization
void Start()
{
_lineRenderer = GetComponent <LineRenderer>();
_lineRenderer.sortingOrder = sortingOrder;
_lineRenderer.sortingLayerName = sortingLayerName;
_ik = GetComponent <IK>();
points = new Vector3[3];
_ik.DidUpdate += MyUpdate;
}
// Use this for initialization
void Start()
{
Load_Command();
Term = Terminal.GetComponent <Terminal>();
TargetControl = GameC.GetComponent <TargetControl>();
CommandControl = GameC.GetComponent <CommandControl>();
Robot = GameC.GetComponent <GameController>().robot;
Online = false;
Data = false;
Count_Command_Data = 0;
}
protected override void UpdateIK()
{
if (!moving)
{
IK.SetIKPositions
(
currentNodes[0].rightHand,
currentNodes[1].leftHand,
currentNodes[0].rightFoot,
currentNodes[1].leftFoot
);
}
}
public void Home(IK robot)
{
// Error: One home does not reach HOME
robot.Home();
robot.Home();
robot.Home();
//
if (gameController.GetOnlineMode())
{
controller.RunCommandOnline("home");
}
}
void Start()
{
// Scorbot inner target
innerTarget = GetComponent <GameController>().innerTarget;
// Camera
cam = GetComponent <GameController>().cam;
// Scorbot
robot = GetComponent <GameController>().robot;
// Controllers
gameController = GetComponent <GameController>();
stateMessageControl = GetComponent <StateMessageControl>();
// Events
GameController.ScorbotDel += SetScorbot;
}
// mm. pos in real Scorbot
public void TeachR(IK robot, Transform target, Transform relativeToTarget, Vector3 pos, float p, float r)
{
Vector3 newPos = new Vector3(relativeToTarget.position.x * 10f, relativeToTarget.position.z * 10f, relativeToTarget.position.y * 10f)
+ pos;
Teach(robot, target, newPos, relativeToTarget.GetComponent <TargetModel>().GetPitch() + p,
(-relativeToTarget.GetComponent <TargetModel>().GetRoll()) + r);
target.GetComponent <TargetModel>().SetRelativeTo(relativeToTarget, new Vector3(pos.x, pos.z, pos.y) / 10f, p, r);
if (gameController.GetOnlineMode())
{
}
}
void Start()
{
// Find the ik component
ik = GetComponent <IK>();
// Workaround for Unity Win Store/Phone serialization bug
stepProgress = 1f;
hit = new RaycastHit();
hit.point = position;
// Store the default rest position of the leg
defaultDirection = mechSpider.transform.TransformDirection(position + offset - mechSpider.transform.position);
}
/**
* Recupera los valores de una posición del Scorbot real en la misma posición en la simulación.
* @param robot Scorbot
* @param target Posición (objeto)
* @return bool Éxito
*/
public bool SyncScorbotToSimulation(IK robot, Transform target)
{
// Only online mode
List <String[]> listString = new List <string[]>();
List <int> counts;
List <float> posPitchRoll;
// This stops main thread
bool listpv = Listpv(target, out counts, out posPitchRoll);
if (listpv)
{
stateMessageControl.WriteMessage("Done. Online SYNC(LISTPV) \"" + target.GetComponent <TargetModel>().GetName() + "\"", listpv);
}
else
{
stateMessageControl.WriteMessage("Error. Online SYNC(LISTPV) \"" + target.GetComponent <TargetModel>().GetName() + "\"", listpv);
return(false);
}
if (robot.GetComponent <ScorbotModel>().scorbotIndex == ScorbotERVPlus.INDEX)
{
for (int i = 0; i < posPitchRoll.Count; i++)
{
posPitchRoll[i] = posPitchRoll[i] / 10f;
}
}
// Do teach only in simulation
bool done = Teach(robot, target, new Vector3(posPitchRoll[0], posPitchRoll[1], posPitchRoll[2]), posPitchRoll[3], posPitchRoll[4], false);
if (done)
{
stateMessageControl.WriteMessage("Done. Online SYNC \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
target.GetComponent <TargetModel>().SetSync(true);
if (target.GetComponent <TargetModel>().GetRelativeFrom())
{
target.GetComponent <TargetModel>().GetRelativeFrom().GetComponent <TargetModel>().SetSync(true);
}
stateMessageControl.UpdatePositionLog();
return(true);
}
else
{
stateMessageControl.WriteMessage("Error. Online SYNC \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
return(false);
}
}
void Awake()
{
// Find the ik component
ik = GetComponent <IK>();
if (foot != null)
{
if (footUpAxis == Vector3.zero)
{
footUpAxis = Quaternion.Inverse(foot.rotation) * Vector3.up;
}
lastFootLocalRotation = foot.localRotation;
ik.GetIKSolver().OnPostUpdate += AfterIK;
}
}
public void ToStreamEx(Stream s, PmxElementFormat f = null)
{
PmxStreamHelper.WriteString(s, Name, f);
PmxStreamHelper.WriteString(s, NameE, f);
V3_BytesConvert.ToStream(s, Position);
PmxStreamHelper.WriteElement_Int32(s, Parent, f.BoneSize);
PmxStreamHelper.WriteElement_Int32(s, Level);
PmxStreamHelper.WriteElement_Int32(s, (int)Flags, 2, signed: false);
if (GetFlag(BoneFlags.ToBone))
{
PmxStreamHelper.WriteElement_Int32(s, To_Bone, f.BoneSize);
}
else
{
V3_BytesConvert.ToStream(s, To_Offset);
}
if (GetFlag(BoneFlags.AddRotation) || GetFlag(BoneFlags.AddTranslation))
{
PmxStreamHelper.WriteElement_Int32(s, AddParent, f.BoneSize);
PmxStreamHelper.WriteElement_Float(s, AddRatio);
}
if (GetFlag(BoneFlags.FixAxis))
{
V3_BytesConvert.ToStream(s, Axis);
}
if (GetFlag(BoneFlags.LocalFrame))
{
if (!f.WithID)
{
NormalizeLocal();
}
V3_BytesConvert.ToStream(s, LocalX);
V3_BytesConvert.ToStream(s, LocalZ);
}
if (GetFlag(BoneFlags.ExtParent))
{
PmxStreamHelper.WriteElement_Int32(s, ExtKey);
}
if (GetFlag(BoneFlags.IK))
{
IK.ToStreamEx(s, f);
}
if (f.WithID)
{
PmxStreamHelper.WriteElement_UInt(s, base.UID);
PmxStreamHelper.WriteElement_UInt(s, base.CID);
}
}
public void FromStreamEx(Stream s, PmxElementFormat f = null)
{
Name = PmxStreamHelper.ReadString(s, f);
NameE = PmxStreamHelper.ReadString(s, f);
Position = V3_BytesConvert.FromStream(s);
Parent = PmxStreamHelper.ReadElement_Int32(s, f.BoneSize);
Level = PmxStreamHelper.ReadElement_Int32(s);
Flags = (BoneFlags)PmxStreamHelper.ReadElement_Int32(s, 2, signed: false);
if (GetFlag(BoneFlags.ToBone))
{
To_Bone = PmxStreamHelper.ReadElement_Int32(s, f.BoneSize);
}
else
{
To_Offset = V3_BytesConvert.FromStream(s);
}
if (GetFlag(BoneFlags.AddRotation) || GetFlag(BoneFlags.AddTranslation))
{
AddParent = PmxStreamHelper.ReadElement_Int32(s, f.BoneSize);
AddRatio = PmxStreamHelper.ReadElement_Float(s);
}
if (GetFlag(BoneFlags.FixAxis))
{
Axis = V3_BytesConvert.FromStream(s);
}
if (GetFlag(BoneFlags.LocalFrame))
{
LocalX = V3_BytesConvert.FromStream(s);
LocalZ = V3_BytesConvert.FromStream(s);
if (!f.WithID)
{
NormalizeLocal();
}
}
if (GetFlag(BoneFlags.ExtParent))
{
ExtKey = PmxStreamHelper.ReadElement_Int32(s);
}
if (GetFlag(BoneFlags.IK))
{
IK.FromStreamEx(s, f);
}
if (f.WithID)
{
base.UID = PmxStreamHelper.ReadElement_UInt(s);
base.CID = PmxStreamHelper.ReadElement_UInt(s);
}
}
/**
* Mueve el Scorbot a su posición HOME.
* @param robot Scorbot
* @return void
*/
public void Home(IK robot)
{
// Offline mode
// Error: One home does not reach HOME
robot.Home();
robot.Home();
robot.Home();
stateMessageControl.WriteMessage("Done. HOME", true);
// Online mode
if (gameController.GetOnlineMode())
{
controller.RunCommandOnline("home");
}
}
