/**
* Aplica el algoritmo CCD para una articulación y la rota.
* @param articulations Articulaciones
* @param D Posición objetivo
* @param E Efector final
* @param R Articulación actual
* @return void
*/
private void CCD(Articulation[] articulations, Transform D, Transform E, Articulation R)
{
// Calculate angle. Radians
float beta1 = 0.0f;
float beta2 = 0.0f;
// Vertical plane
if (R.GetPlane().Equals(PlaneHelper.XY))
{
Vector3 rLocalPos = Vector3.zero;
Vector3 dLocalPos = GetLocalPosition(D, articulations[1].transform);
Vector3 eLocalPos = GetLocalPosition(E, articulations[1].transform);
if (!articulations[1].Equals(R))
{
rLocalPos = GetLocalPosition(R.transform, articulations[1].transform);
}
else
{
rLocalPos = Vector3.zero;
}
beta1 = CalculateAngle(dLocalPos, rLocalPos, R.GetPlane());
beta2 = CalculateAngle(eLocalPos, rLocalPos, R.GetPlane());
}
// Horizontal plane
else
{
beta1 = CalculateAngle(D.position, R.GetPosition(), R.GetPlane());
beta2 = CalculateAngle(E.position, R.GetPosition(), R.GetPlane());
}
float beta = 0.0f;
switch (R.GetPlane())
{
case PlaneHelper.XY:
beta = (beta1 - beta2);
break;
case PlaneHelper.XZ:
beta = (beta2 - beta1);
break;
}
// Shortest path
if (R.GetPlane().Equals(PlaneHelper.XY))
{
while (beta > Mathf.PI)
{
beta -= 2 * Mathf.PI;
}
while (beta < -Mathf.PI)
{
beta += 2 * Mathf.PI;
}
}
// Degrees
beta = beta * Mathf.Rad2Deg;
// Apply angle
R.Rotate(beta);
}
/**
* 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;
}
}
