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 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());
}
}
/**
* 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 void Move(IK robot, Transform target)
{
if (target.GetComponent <TargetModel>().GetAngles() != null)
{
robot.Move(target);
//Debug.Log("Using angles");
}
else
{
robot.CCDAlg(target);
target.GetComponent <TargetModel>().SetAngles(robot.GetAnglesFromCopy());
//Debug.Log("Using IK");
}
//
if (gameController.GetOnlineMode())
{
controller.RunCommandOnline("move" + " " + target.GetComponent <TargetModel>().GetName());
}
}
/**
* Mueve o rota un objeto dependiento del objeto seleccionado. Si es un eje se produce un movimiento
* con ayuda de los cambios del ratón, si es un eje de rotación se aplica una rotación con ayuda
* de los cambios del ratón.
* @return void
*/
private void MoveOrRotate()
{
// Parent from selected object. An object can contain axis and a selectable object,
// these components are contained in a parent object, so to move everything we just move
// the parent since its children will automatically follow.
Transform parent = selectedObject.transform.parent.transform;
// Selected object is an Axis
if (selectedObject.tag.Contains(AXIS_TAG))
{
Vector3 startPos = parent.position;
// Selected object is an Axis X
if (selectedObject.tag.Contains(X_TAG))
{
// Angle between camera and object
float angle = Vector3.Angle(cam.right, parent.right);
// Move object around axis x, angle determines forward or backward
if (angle < 90f)
{
parent.Translate(new Vector3(axisSensibityReduction * axisSensibity *
Input.GetAxis(MOUSE_X), 0f, 0f));
}
else
{
parent.Translate(new Vector3(axisSensibityReduction * -axisSensibity *
Input.GetAxis(MOUSE_X), 0f, 0f));
}
}
// Selected object is an Axis Y
if (selectedObject.tag.Contains(Y_TAG))
{
// Move object around axis y
parent.Translate(new Vector3(0f, axisSensibityReduction * axisSensibity *
Input.GetAxis(MOUSE_Y), 0f));
}
// Selected object is an Axis Z
if (selectedObject.tag.Contains(Z_TAG))
{
// Angle between camera and object
float angle = Vector3.Angle(cam.right, parent.forward);
// Move object around axis z, angle determines forward or backward
if (angle < 90f)
{
parent.Translate(new Vector3(0f, 0f, axisSensibityReduction * axisSensibity *
Input.GetAxis(MOUSE_X)));
}
else
{
parent.Translate(new Vector3(0f, 0f, axisSensibityReduction * -axisSensibity *
Input.GetAxis(MOUSE_X)));
}
}
// Target (a position) moved. Invalid angles
if (selectedObject.tag.Contains(TARGET_TAG) && (!startPos.Equals(parent.position)))
{
// If it is not a being used by another position (relative), this position not sync anymore
if (parent.GetComponent <TargetModel>().GetRelativeTo() == null)
{
Transform relativePosition = parent.GetComponent <TargetModel>().GetRelativeFrom();
parent.GetComponent <TargetModel>().SetSync(false);
// if this position is being used by another position (relative), that position is not sync anymore
;
if (parent.GetComponent <TargetModel>().GetRelativeFrom())
{
relativePosition.GetComponent <TargetModel>().SetSync(false);
// Updating relative position
relativePosition.GetComponent <TargetModel>().UpdateRelativePosition();
// Update angles data
if (robot.TargetInRange(relativePosition))
{
// Reachable. Load data to target
relativePosition.GetComponent <TargetModel>().SetAngles(robot.GetAnglesFromCopy());
relativePosition.GetComponent <TargetModel>().SetValid(true);
}
else // Unreachable
{
relativePosition.GetComponent <TargetModel>().SetValid(false);
}
}
// Check if it's an unreachable point
if (robot.TargetInRange(parent))
{
// Reachable. Load data to target
parent.GetComponent <TargetModel>().SetAngles(robot.GetAnglesFromCopy());
parent.GetComponent <TargetModel>().SetValid(true);
}
else // Unreachable
{
parent.GetComponent <TargetModel>().SetValid(false);
}
stateMessageControl.UpdatePositionLog();
}
else // Moved a relative position, revert position
{
parent.GetComponent <TargetModel>().UpdateRelativePosition();
}
}
// Update trayectory in case a position is moved
gameController.DrawTrayectory();
}
// Selected object is a rotation axis
if (selectedObject.tag.Contains(ROTATION_TAG))
{
// Scorbot articulation
Articulation art = parent.GetComponent <Articulation>();
// If selected object parent is an articulation
if (art != null)
{
// If articulation plane is xz, apply rotation
if (art.GetPlane().Equals(PlaneHelper.XZ))
{
parent.GetComponent <Articulation>().Rotate(-rotationSensibity * Input.GetAxis(MOUSE_X));
}
// If articulation plane is xy, apply rotation
if (art.GetPlane().Equals(PlaneHelper.XY))
{
// Angle between camera and object
float angle = Vector3.Angle(cam.forward, parent.forward);
// Rotate object around plane xy, angle determines forward or backward
if (angle < 90f)
{
parent.GetComponent <Articulation>().Rotate(-rotationSensibity * Input.GetAxis(MOUSE_X));
}
else
{
parent.GetComponent <Articulation>().Rotate(rotationSensibity * Input.GetAxis(MOUSE_X));
}
}
// If articulation plane is yz, apply rotation
if (art.GetPlane().Equals(PlaneHelper.YZ))
{
// Angle between camera and object
float angle = Vector3.Angle(cam.forward, parent.right);
// Rotate object around plane yz, angle determines forward or backward
if (angle < 90f)
{
parent.GetComponent <Articulation>().Rotate(-rotationSensibity * Input.GetAxis(MOUSE_X));
}
else
{
parent.GetComponent <Articulation>().Rotate(rotationSensibity * Input.GetAxis(MOUSE_X));
}
}
}
}
// If selected object is Scorbot InnerAxis (InnerTarget contains InnerAxis)
if (selectedObject.tag.Contains(INNERAXIS_TAG))
{
// Move Scorbot to InnerTarget position
robot.CCDAlg(innerTarget.transform);
}
else // Update InnerTarget position to Scorbot end effector position when is not selected
{
innerTarget.transform.parent.transform.position = robot.GetE().position;
}
}
/**
* Mueve el Scorbot a una posición ya definida pasando por otra posición. El movimiento es una curva generada
* por el algoritmo del spline de CatmullRom.
* @param robot Scorbot
* @param finalPoint Posición final(objeto)
* @param middlePoint Posición intermedia (objeto)
* @return void
*/
public void MoveC(IK robot, Transform finalPoint, Transform middlePoint)
{
// Valid final position
if (!finalPoint.GetComponent <TargetModel>().GetValid())
{
stateMessageControl.WriteMessage("Error. MOVEC Unreachable position \"" + finalPoint.GetComponent <TargetModel>().GetName() + "\"", false);
return;
}
// Valid intermediate position
if (!middlePoint.GetComponent <TargetModel>().GetValid())
{
stateMessageControl.WriteMessage("Error. MOVEC Unreachable position \"" + middlePoint.GetComponent <TargetModel>().GetName() + "\"", false);
return;
}
List <Transform> list = new List <Transform>();
// Scorbot final effector
list.Add(robot.GetE());
// Intermadiate position
list.Add(middlePoint);
// Final position
list.Add(finalPoint);
// Add 2 control points
Vector3[] positions = spline.PrepareSpline(list);
// Get final trajectory
List <Vector3> points = spline.GetTrayectory(positions);
// Build objects from final trajectory
Transform[] trayectory = new Transform[points.Count - 1]; // skip first one, Effector
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;
// Move Scorbot following trajectory
robot.CCDAlg(trayectory, false);
stateMessageControl.WriteMessage("Done. MOVEC \"" + finalPoint.GetComponent <TargetModel>().GetName() + "\"" +
" \"" + middlePoint.GetComponent <TargetModel>().GetName() + "\"", true);
// Online mode
if (gameController.GetOnlineMode())
{
bool done = controller.RunCommandUIOnline("movec", finalPoint.GetComponent <TargetModel>().GetName(), middlePoint.GetComponent <TargetModel>().GetName());
if (done)
{
string aux = "";
if (!finalPoint.GetComponent <TargetModel>().GetSync() || !middlePoint.GetComponent <TargetModel>().GetSync())
{
aux = ". NO SYNC";
}
stateMessageControl.WriteMessage("Done. Online MOVEC \"" + finalPoint.GetComponent <TargetModel>().GetName() + "\"" +
" \"" + middlePoint.GetComponent <TargetModel>().GetName() + aux, done);
}
else
{
stateMessageControl.WriteMessage("Error. Online MOVEC \"" + finalPoint.GetComponent <TargetModel>().GetName() + "\"" +
" \"" + middlePoint.GetComponent <TargetModel>().GetName() + "\"", done);
}
}
}
