en otra ventana

193
APÉNDICE D. CÓDIGO DEL SIMULADOR CINEMÁTICO
El código en Java para el simulador cinemático se presenta en este apartado, siendo cada
entidad de código (clase o interfaz) colocada en su respectiva sección.
D.1. Clase Tesis3
import java.awt.*;
import java.awt.event.*;
import javax.swing.*;
import javax.swing.border.*;
public class Tesis3 {
// Version 3.0 del simulador
UIManager.setLookAndFeel("com.sun.java.swing.plaf.windows.Wind
owsLookAndFeel");
}
catch (Exception ex) {
System.out.println("Failed loading L&F: ");
System.out.println(ex);
}
public static void main(String[] args) {
Tesis3Frame frame = new Tesis3Frame();
// Creación del frame
JFrame.setDefaultLookAndFeelDecorated (true);
JDialog.setDefaultLookAndFeelDecorated (true);
// Mostrar frame
frame.setVisible(true);
}
}
try {
D.2. Clase Tesis3Frame
import java.awt.*;
import java.awt.event.*;
import java.io.IOException;
import javax.swing.*;
import javax.swing.border.*;
public class Tesis3Frame extends JFrame { // Version 3.0 del simulador
try {
controls = new Controls (parallelRobot, pol,
opengl_canvas);
} catch (IOException e) {
System.err.println();
System.err.println(e.getMessage());
System.exit(1);
}
// Declaración de componentes
simulator.add (controls, BorderLayout.CENTER);
simulator.add (opengl_canvas, BorderLayout.EAST);
contentPane.setLayout (new BorderLayout());
contentPane.add (simulator, BorderLayout.NORTH);
private Container contentPane;
Plataform parallelRobot;
Polygonizer pol;
OpenGLCanvas opengl_canvas;
Controls controls;
// Declaración de listeners
menuFileExit.addActionListener (
new ActionListener() {
public void actionPerformed(ActionEvent e) {
Tesis3Frame.this.windowClosed();
}
}
);
// Constructor
public Tesis3Frame() {
contentPane = this.getContentPane();
JPanel simulator = new JPanel (new BorderLayout ());
JMenuBar menuBar = new JMenuBar();
JMenu menuFile = new JMenu("File");
JMenuItem menuFileExit = new JMenuItem("File");
this.addWindowListener (
new WindowAdapter() {
public void windowClosing (WindowEvent e) {
Tesis3Frame.this.windowClosed();
}
}
);
parallelRobot = new Plataform (11.0, 3.4, 7.68, 1.45, 1.62,
7.86, 71.38, Plataform.DEG);
pol = new Polygonizer(parallelRobot);
pol.start();
opengl_canvas = new OpenGLCanvas (450, 450,
parallelRobot, pol);
setTitle("Tesis");
setSize(900, 500);
}
// Finaliza la aplicación
protected void windowClosed() {
System.exit(0);
}
menuFile.add(menuFileExit);
menuBar.add(menuFile);
setJMenuBar(menuBar);
}
194
D.3. Clase Plataform
public class Plataform implements Implicit {
// Constantes
final static int DEG = 0;
final static int RAD = 1;
final private int PRECISION = 10;
final private int MAX_ITERA = 15;
final private double STEP_0 = 0.125;
final private double sqrt3_6 = Math.sqrt(3) / 6;
final private double [] Sigma = {-2 * Math.PI / 3, -2 * Math.PI / 3, 0, 0, 2 * Math.PI / 3, 2 * Math.PI / 3};
// Propiedades relacionadas con la geometría básica del robot paralelo
private double b, d, a, c;
private double P, L;
// Propiedades relacionadas con la traslación y rotación de la plataforma
private double px, py, pz;
private double alfa, beta, gamma;
// Propiedades relacionadas con la posición actual de los vértices del robot paralelo
private Vector [] Base = new Vector [6];
private Vector [] Top = new Vector [6];
private Vector [] Q = new Vector [6];
// Propiedades relacionadas con los ángulos de las flechas
private double [] Theta = {0, 0, 0, 0, 0, 0};
private int [] optionTheta = {1, 1, 1, 1, 1, 1};
// Propiedades relacionadas con el eslabón L y las rótulas
private double Rot_Max_Angle;
private double [] thetaL = new double [6];
private double [] phiL = new double [6];
private double [] fiL = new double [6];
private Rotula [] elbowRot = new Rotula [6];
private Rotula [] topRot = new Rotula [6];
private boolean inRotWorkspace = true;
// Variables auxiliares para evitar repeticiones en cálculos
private double a_sqrt3, c_sqrt3;
// Constructores
Plataform () {
initializeParameters (5.0, 1.0, 3.0, 1.0, 1.0, 3.0, Math.PI / 4);
setTheta (0, 0, 0, 0, 0, 0, DEG);
}
Plataform (double param_b, double param_d, double param_a, double param_c, double param_p, double param_l, double
param_rot_max_angle, int type) {
initializeParameters (param_b, param_d, param_a, param_c, param_p, param_l, toRad (param_rot_max_angle, type));
setTheta (0, 0, 0, 0, 0, 0, DEG);
}
Plataform (int param_b, int param_d, int param_a, int param_c, int param_p, int param_l, int param_rot_max_angle, int type) {
initializeParameters ((double) param_b, (double) param_d, (double) param_a, (double) param_c, (double) param_p, (double) param_l,
toRad ((double) param_rot_max_angle, type));
setTheta (0, 0, 0, 0, 0, 0, DEG);
}
// Métodos Set
// Relacionados con la traslación de la plataforma
void setPx (double param_px) {
if (inWorkspace (param_px, py, pz, alfa, beta, gamma)) {
if (px != param_px)
px = param_px;
calculateInverseKinematics ();
}
}
195
void setPx (int param_px) {
setPx ((double) param_px);
}
void setPy (double param_py) {
if (inWorkspace (px, param_py, pz, alfa, beta, gamma)) {
if (py != param_py)
py = param_py;
calculateInverseKinematics ();
}
}
void setPy (int param_py) {
setPy ((double) param_py);
}
void setPz (double param_pz) {
if (inWorkspace (px, py, param_pz, alfa, beta, gamma)) {
if (pz != param_pz)
pz = param_pz;
calculateInverseKinematics ();
}
}
void setPz (int param_pz) {
setPz ((double) param_pz);
}
void setPosition (double param_px, double param_py, double param_pz) {
if (inWorkspace (param_px, param_py, param_pz, alfa, beta, gamma)) {
if (px != param_px)
px = param_px;
if (py != param_py)
py = param_py;
if (pz != param_pz)
pz = param_pz;
calculateInverseKinematics ();
}
}
void setPosition (int param_px, int param_py, int param_pz) {
setPosition ((double) param_px, (double) param_py, (double) param_pz);
}
// Relacionados con la rotación de la plataforma
void setAlfa (double param_alfa, int type) {
double alfa_temp = toRad (param_alfa, type);
if (inWorkspace (px, py, pz, alfa_temp, beta, gamma)) {
if (alfa != alfa_temp)
alfa = alfa_temp;
calculateInverseKinematics ();
}
}
void setAlfa (int param_alfa, int type) {
setAlfa ((double) param_alfa, type);
}
void setBeta (double param_beta, int type) {
double beta_temp = toRad (param_beta, type);
if (inWorkspace (px, py, pz, alfa, beta_temp, gamma)) {
if (beta != beta_temp)
beta = beta_temp;
calculateInverseKinematics ();
196
}
}
void setBeta (int param_beta, int type) {
setBeta ((double) param_beta, type);
}
void setGamma (double param_gamma, int type) {
double gamma_temp = toRad (param_gamma, type);
if (inWorkspace (px, py, pz, alfa, beta, gamma_temp)) {
if (gamma != gamma_temp)
gamma = gamma_temp;
calculateInverseKinematics ();
}
}
void setGamma (int param_gamma, int type) {
setGamma ((double) param_gamma, type);
}
void setOrientation (double param_alfa, double param_beta, double param_gamma, int type) {
double alfa_temp = toRad (param_alfa, type);
double beta_temp = toRad (param_beta, type);
double gamma_temp = toRad (param_gamma, type);
if (inWorkspace (px, py, pz, alfa_temp, beta_temp, gamma_temp)) {
if (alfa != alfa_temp)
alfa = alfa_temp;
if (beta != beta_temp)
beta = beta_temp;
if (gamma != gamma_temp)
gamma = gamma_temp;
calculateInverseKinematics ();
}
}
void setOrientation (int param_alfa, int param_beta, int param_gamma, int type) {
setOrientation ((double) param_alfa, (double) param_beta, (double) param_gamma, type);
}
// Relacionados con los ángulos de las flechas
void setTheta (double param_theta, int num, int type) {
double theta_temp = toRad (param_theta, type);
if (Theta [num - 1] != theta_temp)
Theta [num - 1] = theta_temp;
calculateDirectKinematics ();
}
void setTheta (int param_theta, int num, int type) {
setTheta ((double) param_theta, num, type);
}
void setTheta (double [] param_theta, int type) {
double theta_temp;
for (int j = 0; j < 6; j++)
{
theta_temp = toRad (param_theta[j], type);
if (Theta[j] != theta_temp) {
if (j < param_theta.length)
Theta [j] = theta_temp;
else
Theta [j] = 0;
}
}
calculateDirectKinematics ();
}
197
void setTheta (double p_t1, double p_t2, double p_t3, double p_t4, double p_t5, double p_t6, int type) {
double [] p_Theta = {p_t1, p_t2, p_t3, p_t4, p_t5, p_t6};
setTheta (p_Theta, type);
}
void setTheta (int p_t1, int p_t2, int p_t3, int p_t4, int p_t5, int p_t6, int type) {
double [] p_Theta = {(double) p_t1, (double) p_t2, (double) p_t3, (double) p_t4, (double) p_t5, (double) p_t6};
setTheta (p_Theta, type);
}
return new Vector (px, py, pz);
void setThetaOption (int num, int opt) {
}
if ((num >= 1 && num <= 6) && (opt == 1 || opt == 2))
optionTheta [num - 1] = opt;
calculateDirectKinematics ();
double getAlfa (int type) {
return convertAngle (alfa, type);
}
}
void setThetaOption (int [] options) {
double getBeta (int type) {
return convertAngle (beta, type);
}
for (int j = 0; j < 6; j++) {
if (options [j] == 1 || options [j] == 2) {
if (j < options.length)
optionTheta [j] = options [j];
else
optionTheta [j] = 1;
double getGamma (int type) {
return convertAngle (gamma, type);
}
// Relacionados con los ángulos de las flechas
double [] getTheta (int type) {
}
}
double [] Theta_temp = new double [6];
calculateDirectKinematics ();
for (int j = 0; j < 6; j++)
Theta_temp[j] = convertAngle(Theta[j], type);
}
void setThetaOption (int opt1, int opt2, int opt3, int opt4,
int opt5, int opt6) {
int [] options = {opt1, opt2, opt3, opt4, opt5, opt6};
setThetaOption (options);
return Theta_temp;
}
double getTheta (int num, int type) {
return convertAngle(Theta [num - 1], type);
}
}
// Métodos Get
int [] getOptionTheta () {
return optionTheta;
}
// Relacionados con la geometría de la plataforma
double getB () {
return b;
}
int getOptionTheta (int num) {
return optionTheta [num - 1];
}
// Relacionados con la geometría básica
double getD () {
return d;
}
double getA () {
return a;
}
double getC () {
return c;
}
double getP () {
return P;
}
double getL () {
return L;
}
double getSigma (int num, int type) {
return convertAngle(Sigma [num - 1], type);
}
// Relacionados con la posición y orientación de la plataforma
double getPx () {
return px;
}
double getPy () {
return py;
}
double getPz () {
return pz;
}
Vector getPosition () {
Vector getBase (int num) {
return Base [num - 1];
}
Vector getTop (int num) {
return Top [num - 1];
}
Vector getQ (int num) {
return Q [num - 1];
}
198
return angle_temp;
// Relacionados con el eslabón L y las rótulas
}
double getRotMaxAngle () {
return Rot_Max_Angle;
}
private double convertAngle (double angle, int type) {
double angle_temp = 0;
double getThetaL (int num, int type) {
return convertAngle(thetaL [num - 1], type);
}
switch (type) {
case DEG:
angle_temp = angle * 180 / Math.PI;
break;
double getPhiL (int num, int type) {
return convertAngle(phiL [num - 1], type);
}
case RAD:
angle_temp = angle;
break;
double getFiL (int num, int type) {
return convertAngle(fiL [num - 1], type);
}
}
return angle_temp;
Rotula getElbowRot (int num) {
return elbowRot [num - 1];
}
Rotula getTopRot (int num) {
return topRot [num - 1];
}
boolean getInRotWorkspace () {
return inRotWorkspace;
}
}
private double trunc (double x, int numDec) {
return ((double) Math.round(x * Math.pow(10, numDec))) /
Math.pow(10, numDec);
}
// Funciones específicas relacionadas con los vértices
private void calculateBaseVertex () {
Base [0] = new Vector ( sqrt3_6 * (2*b + d), d/2, 0);
Base [1] = new Vector (-sqrt3_6 * (b - d), (b + d)/2, 0);
Base [2] = new Vector (-sqrt3_6 * (b + 2*d), b/2, 0);
Base [3] = new Vector (-sqrt3_6 * (b + 2*d), -b/2, 0);
Base [4] = new Vector (-sqrt3_6 * (b - d), -(b + d)/2, 0);
Base [5] = new Vector ( sqrt3_6 * (2*b + d), -d/2, 0);
// Funciones generales
private void initializeParameters (double param_b, double
param_d, double param_a, double param_c, double param_p, double
param_l, double param_rot_max_angle) {
b = param_b;
d = param_d;
a = param_a;
c = param_c;
P = param_p;
L = param_l;
Rot_Max_Angle = param_rot_max_angle;
// Variables auxiliares
a_sqrt3 = a / Math.sqrt(3);
c_sqrt3 = c / Math.sqrt(3);
}
private void calculateTopVertex () {
Top = calculateEachTopVertex (px, py, pz, alfa, beta,
gamma);
}
private Vector [] calculateEachTopVertex (double p_x, double
p_y, double p_z, double p_alfa, double p_beta, double p_gamma) {
Vector [] Top_t = new Vector [6];
// Declaración y cálculo de las variables auxiliares
// Inicialización de las rótulas
for (int j = 0; j < 6; j++) {
elbowRot [j] = new Rotula ();
topRot [j] = new Rotula ();
}
// Inicialización de los vértices
calculateBaseVertex ();
calculateTopVertex ();
}
private double toRad (double angle, int type) {
double angle_temp = 0;
switch (type) {
case DEG:
angle_temp = angle * Math.PI / 180.0;
break;
case RAD:
angle_temp = angle;
break;
double cA = Math.cos(p_alfa);
double cB = Math.cos(p_beta);
double cG = Math.cos(p_gamma);
double cG60 = Math.cos(p_gamma + Math.PI / 3);
double cG30 = Math.cos(p_gamma + Math.PI / 6);
double sA = Math.sin(p_alfa);
double sB = Math.sin(p_beta);
double sG = Math.sin(p_gamma);
double sG60 = Math.sin(p_gamma + Math.PI / 3);
double sG30 = Math.sin(p_gamma + Math.PI / 6);
double cBcG = cB * cG;
double cBcG60 = cB * cG60;
double cBsG30 = cB * sG30;
double cAsG = cA * sG;
double cAsG60 = cA * sG60;
double cAcG30 = cA * cG30;
double sBcG = sB * cG;
double sBcG60 = sB * cG60;
double sAsBcG30 = sA * sB * cG30;
double sAsBsG = sA * sB * sG;
double sAsBsG60 = sA * sB * sG60;
double sBsG30 = sB * sG30;
double sAcBsG = sA * cB * sG;
double sAcBsG60 = sA * cB * sG60;
double sAcBcG30 = sA * cB * cG30;
}
// Cálculo de los vértices
Top_t [0] = new Vector (
a_sqrt3 * (cBcG60 + sAsBsG60) + c_sqrt3 * (cBcG + sAsBsG) + p_x,
a_sqrt3 * cAsG60 + c_sqrt3 * cAsG + p_y,
a_sqrt3 * (-sBcG60 + sAcBsG60) + c_sqrt3 * (-sBcG + sAcBsG) + p_z
);
199
Top_t [1] = new Vector (
a_sqrt3 * (cBcG60 + sAsBsG60) + c_sqrt3 * (-cBsG30 + sAsBcG30) + p_x,
a_sqrt3 * cAsG60 + c_sqrt3 * cAcG30 + p_y,
a_sqrt3 * (-sBcG60 + sAcBsG60) + c_sqrt3 * (sBsG30 + sAcBcG30) + p_z
);
Top_t [2] = new Vector (
-a_sqrt3 * (cBcG + sAsBsG) + c_sqrt3 * (-cBsG30 + sAsBcG30) + p_x,
-a_sqrt3 * cAsG + c_sqrt3 * cAcG30 + p_y,
-a_sqrt3 * (-sBcG + sAcBsG) + c_sqrt3 * (sBsG30 + sAcBcG30) + p_z
);
Top_t [3] = new Vector (
-a_sqrt3 * (cBcG + sAsBsG) + c_sqrt3 * (-cBcG60 - sAsBsG60) + p_x,
-a_sqrt3 * cAsG + c_sqrt3 * -cAsG60 + p_y,
-a_sqrt3 * (-sBcG + sAcBsG) + c_sqrt3 * (sBcG60 - sAcBsG60) + p_z
);
Top_t [4] = new Vector (
a_sqrt3 * (cBsG30 - sAsBcG30) + c_sqrt3 * (-cBcG60 - sAsBsG60) + p_x,
a_sqrt3 * -cAcG30 + c_sqrt3 * -cAsG60 + p_y,
a_sqrt3 * (-sBsG30 - sAcBcG30) + c_sqrt3 * (sBcG60 - sAcBsG60) + p_z
);
Top_t [5] = new Vector (
a_sqrt3 * (cBsG30 - sAsBcG30) + c_sqrt3 * (cBcG + sAsBsG) + p_x,
a_sqrt3 * -cAcG30 + c_sqrt3 * cAsG + p_y,
a_sqrt3 * (-sBsG30 - sAcBcG30) + c_sqrt3 * (-sBcG + sAcBsG) + p_z
);
return Top_t;
}
private void calculateQVertex () {
for (int j = 0; j < 6; j++)
Q[j] = new Vector (-P * Math.sin(Sigma[j]) * Math.sin(Theta[j]) + Base[j].x, P * Math.cos(Sigma[j]) * Math.sin(Theta[j]) + Base[j].y, -P
* Math.cos(Theta[j]));
}
// Funciones específicas relacionadas con el espacio de trabajo
private double [] Workspace_functions (double p_x, double p_y, double p_z, double p_alfa, double p_beta, double p_gamma) {
double A, B, C;
double [] f = new double [6];
Vector [] Top_t = calculateEachTopVertex (p_x, p_y, p_z, p_alfa, p_beta, p_gamma);
for (int j = 0; j < 6; j++) {
A = 2 * P * (Math.sin(Sigma[j]) * (Top_t[j].x - Base[j].x) - Math.cos(Sigma[j]) * (Top_t[j].y - Base[j].y));
B = 2 * P * Top_t[j].z;
C = Math.pow(L, 2) - Math.pow(P, 2) - Math.pow(Top_t[j].x - Base[j].x, 2) - Math.pow(Top_t[j].y - Base[j].y, 2) - Math.pow(Top_t[j].z,
2);
f[j] = Math.pow(C, 2) - Math.pow(A, 2) - Math.pow(B, 2);
}
return f;
}
private boolean inWorkspace (double p_x, double p_y, double p_z, double p_alfa, double p_beta, double p_gamma) {
double [] f = Workspace_functions (p_x, p_y, p_z, p_alfa, p_beta, p_gamma);
boolean in = true;
for (int j = 0; j < 6; j++)
in = in && (f[j] <= 0);
return in;
}
public double f_value (Vector p) {
double [] f = Workspace_functions (p.x, p.y, p.z, alfa, beta, gamma);
double g = f[0];
for (int j = 1; j < 6; j++)
g = Math.max (f[j], g);
return g;
}
public Vector f_ngrad (Vector p) {
200
double A, B, C;
double par_px, par_py, par_pz;
double [] f = Workspace_functions (p.x, p.y, p.z, alfa, beta, gamma);
int max = 0;
for (int j = 1; j < 6; j++)
max = f[j] > f[max] ? j : max;
Vector [] Top_t = calculateEachTopVertex (p.x, p.y, p.z, alfa, beta, gamma);
A = 2 * P * (Math.sin(Sigma[max]) * (Top_t[max].x - Base[max].x) - Math.cos(Sigma[max]) * (Top_t[max].y - Base[max].y));
B = 2 * P * Top_t[max].z;
C = Math.pow(L, 2) - Math.pow(P, 2) - Math.pow(Top_t[max].x - Base[max].x, 2) - Math.pow(Top_t[max].y - Base[max].y, 2) Math.pow(Top_t[max].z, 2);
par_px = -4 * (Top_t[max].x - Base[max].x) * C - 4 * P * Math.sin(Sigma[max]) * A;
par_py = -4 * (Top_t[max].y - Base[max].y) * C + 4 * P * Math.cos(Sigma[max]) * A;
par_pz = -4 * Top_t[max].z * C - 4 * P * B;
Vector grad = new Vector (par_px, par_py, par_pz);
return Vector.scalev (1.0 / Vector.norm (grad), grad);
}
// Funciones específicas relacionadas con el eslabón L y las rótulas
private void calculateLAngles () {
Vector [] LUnitVec = new Vector [6];
double fiLmin, fiLmax, fiLtemp;
Vector [] vecAnglesRot = new Vector [6];
inRotWorkspace = true;
for (int j = 0; j < 6; j++) {
LUnitVec[j] = Vector.scalev (1 / L ,Vector.diffv (Top[j], Q[j]));
phiL[j] = Math.atan2 (LUnitVec[j].y, LUnitVec[j].x);
thetaL[j] = Math.acos (LUnitVec[j].z);
fiLmin = calculateFiElbow (phiL[j], thetaL[j], Sigma[j]) [0];
fiLtemp = calculateFiTop (phiL[j], thetaL[j], Sigma[j]) [0];
if (Double.isNaN(fiLmin) || Double.isNaN(fiLtemp))
inRotWorkspace = false;
if (fiLmin < fiLtemp)
fiLmin = fiLtemp;
fiLmax = calculateFiElbow (phiL[j], thetaL[j], Sigma[j]) [1];
fiLtemp = calculateFiTop (phiL[j], thetaL[j], Sigma[j]) [1];
if (Double.isNaN(fiLmax) || Double.isNaN(fiLtemp))
inRotWorkspace = false;
if (fiLmax > fiLtemp)
fiLmax = fiLtemp;
if (inRotWorkspace) {
fiL[j] = (fiLmin + fiLmax) / 2;
}
else {
fiL[j] = Double.NaN;
}
}
}
private double [] calculateFiElbow (double phi_rot, double theta_rot, double sigma) {
double A, Bs, Bc, C;
double argum_cos, angle_tan;
double [] fi_rot = new double [4];
double cP = Math.cos(phi_rot);
double cT = Math.cos(theta_rot);
double cS = Math.cos(sigma);
double sP = Math.sin(phi_rot);
double sT = Math.sin(theta_rot);
double sS = Math.sin(sigma);
A = sP * sT * sS + cP * sT * cS;
Bs = -sP * cT * sS - cP * cT * cS;
Bc = -sP * cS + cP * sS;
C = Math.cos(Rot_Max_Angle);
argum_cos = Math.sqrt((Math.pow(C, 2) - Math.pow(A, 2)) / (Math.pow(Bs, 2) + Math.pow(Bc, 2)));
angle_tan = Math.atan2(Bs, Bc);
201
fi_rot[0] = (float) -Math.acos(-argum_cos) + angle_tan;
fi_rot[1] = (float) -Math.acos(argum_cos) + angle_tan;
fi_rot[2] = (float) Math.acos(argum_cos) + angle_tan;
fi_rot[3] = (float) Math.acos(-argum_cos) + angle_tan;
return fi_rot;
}
private double [] calculateFiTop (double phi_rot, double theta_rot, double sigma) {
double A, Bs, Bc, C;
double argum_cos, angle_tan;
double [] fi_rot = new double [4];
double cA = Math.cos(alfa);
double cB = Math.cos(beta);
double cG = Math.cos(gamma);
double cP = Math.cos(phi_rot);
double cT = Math.cos(theta_rot);
double cS = Math.cos(sigma);
double sA = Math.sin(alfa);
double sB = Math.sin(beta);
double sG = Math.sin(gamma);
double sP = Math.sin(phi_rot);
double sT = Math.sin(theta_rot);
double sS = Math.sin(sigma);
double cAcG = cA * cG;
double cAsG = cA * sG;
double cBcG = cB * cG;
double cBsG = cB * sG;
double sBcG = sB * cG;
double sBsG = sB * sG;
double sAcBcG = sA * cB * cG;
double sAcBsG = sA * cB * sG;
double sAsBcG = sA * sB * cG;
double sAsBsG = sA * sB * sG;
double cPcT = cP * cT;
double cPsT = cP * sT;
double sPcT = sP * cT;
double sPsT = sP * sT;
A = sPsT * (cAsG * cS + cAcG * sS) + cPsT * (cBcG * cS + sAsBsG * cS - cBsG * sS + sAsBcG * sS) + cT * (-sBcG * cS + sAcBsG * cS
+ sBsG * sS + sAcBcG * sS);
Bs = sPcT * (-cAcG * sS - cAsG * cS) + cPcT * (cBsG * sS - cBcG * cS - sAsBcG * sS - sAsBsG * cS) + sT * (sBsG * sS - sBcG * cS +
sAcBcG * sS + sAcBsG * cS);
Bc = sP * (cBsG * sS - cBcG * cS - sAsBcG * sG - sAsBsG * cS) + cP * (cAsG * cS + cAcG * sS);
C = Math.cos(Rot_Max_Angle);
argum_cos = Math.sqrt((Math.pow(C, 2) - Math.pow(A, 2)) / (Math.pow(Bs, 2) + Math.pow(Bc, 2)));
angle_tan = Math.atan2(Bs, Bc);
fi_rot[0] = (float) -Math.acos(-argum_cos) + angle_tan;
fi_rot[1] = (float) -Math.acos(argum_cos) + angle_tan;
fi_rot[2] = (float) Math.acos(argum_cos) + angle_tan;
fi_rot[3] = (float) Math.acos(-argum_cos) + angle_tan;
return fi_rot;
}
private void calculateRotState () {
double cA = Math.cos(alfa);
double cB = Math.cos(beta);
double cG = Math.cos(gamma);
double sA = Math.sin(alfa);
double sB = Math.sin(beta);
double sG = Math.sin(gamma);
double cAsB = cA * sB;
double cAcB = cA * cB;
double cAsG = cA * sG;
double cAcG = cA * cG;
double cBsG = cB * sG;
double cBcG = cB * cG;
double sBsG = sB * sG;
double sBcG = sB * cG;
double sAsBsG = sA * sB * sG;
double sAsBcG = sA * sB * cG;
double sAcBsG = sA * cB * sG;
double sAcBcG = sA * cB * cG;
double cS, sS;
for (int j = 0; j < 6; j++) {
cS = Math.cos(Sigma[j]);
sS = Math.sin(Sigma[j]);
elbowRot[j].nx = new Vector (Math.cos(Sigma[j]), Math.sin(Sigma[j]), 0);
202
elbowRot[j].ny = new Vector (-Math.sin(Sigma[j]), Math.cos(Sigma[j]), 0);
elbowRot[j].nz = new Vector (0, 0, 1);
topRot[j].nx = new Vector (cS * (cBcG + sAsBsG) + sS * (-cBsG + sAsBcG), cS * (cAsG) + sS * (cAcG), cS * (-sBcG + sAcBsG) +
sG * (sBsG + sAcBcG));
topRot[j].ny = new Vector (-sS * (cBcG + sAsBsG) + cS * (-cBsG + sAsBcG), -sS * (cAsG) + cS * (cAcG), -sS * (-sBcG + sAcBsG) +
cS * (sBsG + sAcBcG));
topRot[j].nz = new Vector (cAsB, -sA, cAcB);
}
}
// Funciones específicas relacionadas con la cinemática
private void calculateInverseKinematics () {
double A, B, C, angle_cos, angle_tan;
calculateTopVertex ();
for (int j = 0; j < 6; j++) {
A = 2 * P * (Math.sin(Sigma[j]) * (Top[j].x - Base[j].x) - Math.cos(Sigma[j]) * (Top[j].y - Base[j].y));
B = 2 * P * Top[j].z;
C = Math.pow(L, 2) - Math.pow(P, 2) - Math.pow(Top[j].x - Base[j].x, 2) - Math.pow(Top[j].y - Base[j].y, 2) - Math.pow(Top[j].z, 2);
angle_cos = Math.acos(C / Math.sqrt(Math.pow(A, 2) + Math.pow(B, 2)));
angle_tan = Math.atan2 (A, B);
switch (optionTheta[j]) {
case 1:
Theta[j] = angle_cos + angle_tan;
break;
case 2:
Theta[j] = -angle_cos + angle_tan;
break;
}
}
calculateQVertex ();
calculateLAngles ();
calculateRotState ();
while ((norma > Math.pow(10, -PRECISION)) &&
(count_itera <= MAX_ITERA)) {
// Cálculo de las variables auxiliares principales
cA = Math.cos(p_alfa);
cB = Math.cos(p_beta);
cG = Math.cos(p_gamma);
cG60 = Math.cos(p_gamma + Math.PI / 3);
cG30 = Math.cos(p_gamma + Math.PI / 6);
sA = Math.sin(p_alfa);
sB = Math.sin(p_beta);
sG = Math.sin(p_gamma);
sG60 = Math.sin(p_gamma + Math.PI / 3);
sG30 = Math.sin(p_gamma + Math.PI / 6);
}
// Cálculo de las variables auxiliares
private void calculateDirectKinematics () {
// Declaración de variables auxiliares
double cA, cB, cG, cG60, cG30;
double sA, sB, sG, sG60, sG30;
double cBcG, cBcG60, cBsG30;
double cAsG, cAsG60, cAcG30;
double sBcG, sBcG60, sAsBcG30;
double sAsBsG, sAsBsG60, sBsG30;
double sAcBsG, sAcBsG60, sAcBcG30;
double cBsG, cBsG60, cBcG30;
double cAcG, cAcG60, cAsG30;
double sBsG, sBsG60, sBcG30;
double sAsG, sAsG60, sAcG30;
double cAsBsG, cAsBsG60, cAsBcG30;
double sAsBcG, sAsBcG60, sAsBsG30;
double cAcBsG, cAcBsG60, cAcBcG30;
double sAcBcG, sAcBcG60, sAcBsG30;
Vector [] Top_t = new Vector [6];
double [] f = new double [6];
double [] fx = new double [6];
double [] fy = new double [6];
double [] fz = new double [6];
double [] falfa = new double [6];
double [] fbeta = new double [6];
double [] fgamma = new double [6];
// Inicialización
double p_x = px;
double p_y = py;
double p_z = pz;
double p_alfa = alfa;
double p_beta = beta;
double p_gamma = gamma;
double norma = 1;
double count_itera = 0;
// Iteraciones
cBcG = cB * cG;
cBcG60 = cB * cG60;
cBsG30 = cB * sG30;
cAsG = cA * sG;
cAsG60 = cA * sG60;
cAcG30 = cA * cG30;
sBcG = sB * cG;
sBcG60 = sB * cG60;
sAsBcG30 = sA * sB * cG30;
sAsBsG = sA * sB * sG;
sAsBsG60 = sA * sB * sG60;
sBsG30 = sB * sG30;
sAcBsG = sA * cB * sG;
sAcBsG60 = sA * cB * sG60;
sAcBcG30 = sA * cB * cG30;
cBsG = cB * sG;
cBsG60 = cB * sG60;
cBcG30 = cB * cG30;
cAcG = cA * cG;
cAcG60 = cA * cG60;
cAsG30 = cA * sG30;
sBsG = sB * sG;
sBsG60 = sB * sG60;
sBcG30 = sB * cG30;
sAsG = sA * sG;
sAsG60 = sA * sG60;
sAcG30 = sA * cG30;
cAsBsG = cA * sB * sG;
cAsBsG60 = cA * sB * sG60;
cAsBcG30 = cA * sB * cG30;
sAsBcG = sA * sB * cG;
sAsBcG60 = sA * sB * cG60;
sAsBsG30 = sA * sB * sG30;
cAcBsG = cA * cB * sG;
cAcBsG60 = cA * cB * sG60;
cAcBcG30 = cA * cB * cG30;
sAcBcG = sA * cB * cG;
sAcBcG60 = sA * cB * cG60;
sAcBsG30 = sA * cB * sG30;
203
// Cálculo de los vértices temporales
Top_t = calculateEachTopVertex (p_x, p_y, p_z, p_alfa, p_beta, p_gamma);
// Cálculo de las variables necesarias para Newton-Raphson
for (int j = 0; j < 6; j++) {
double ffx = (Top_t[j].x + P * Math.sin(Sigma[j]) * Math.sin(Theta[j]) - Base[j].x);
double ffy = (Top_t[j].y - P * Math.cos(Sigma[j]) * Math.sin(Theta[j]) - Base[j].y);
double ffz = (Top_t[j].z + P * Math.cos(Theta[j]));
f [j] = Math.pow (ffx, 2) + Math.pow (ffy, 2) + Math.pow (ffz, 2) - Math.pow(L, 2);
fx [j] = 2 * ffx;
fy [j] = 2 * ffy;
fz [j] = 2 * ffz;
}
falfa[0] = fx[0] * (a_sqrt3 * cAsBsG60 + c_sqrt3 * cAsBsG) + fy[0] * (-a_sqrt3 * sAsG60 - c_sqrt3 * sAsG) + fz[0] * (a_sqrt3 *
cAcBsG60 + c_sqrt3 * cAcBsG);
fbeta[0] = fx[0] * (a_sqrt3 * (-sBcG60 + sAcBsG60) + c_sqrt3 * (-sBcG + sAcBsG)) + fz[0] * (-a_sqrt3 * (cBcG60 + sAsBsG60) c_sqrt3 * (cBcG + sAsBsG));
fgamma[0] = fx[0] * (a_sqrt3 * (-cBsG60 + sAsBcG60) + c_sqrt3 * (-cBsG + sAsBcG)) + fy[0] * (a_sqrt3 * cAcG60 + c_sqrt3 * cAcG)
+ fz[0] * (a_sqrt3 * (sBsG60 + sAcBcG60) + c_sqrt3 * (sBsG + sAcBcG));
falfa[1] = fx[1] * (a_sqrt3 * cAsBsG60 + c_sqrt3 * cAsBcG30) + fy[1] * (-a_sqrt3 * sAsG60 - c_sqrt3 * sAcG30) + fz[1] * (a_sqrt3 *
cAcBsG60 + c_sqrt3 * cAcBcG30);
fbeta[1] = fx[1] * (a_sqrt3 * (-sBcG60 + sAcBsG60) + c_sqrt3 * (sBsG30 + sAcBcG30)) + fz[1] * (-a_sqrt3 * (cBcG60 + sAsBsG60)
+ c_sqrt3 * (cBsG30 - sAsBcG30));
fgamma[1] = fx[1] * (a_sqrt3 * (-cBsG60 + sAsBcG60) - c_sqrt3 * (cBcG30 + sAsBsG30)) + fy[1] * (a_sqrt3 * cAcG60 - c_sqrt3 *
cAsG30) + fz[1] * (a_sqrt3 * (sBsG60 + sAcBcG60) + c_sqrt3 * (sBcG30 - sAcBsG30));
falfa[2] = fx[2] * (-a_sqrt3 * cAsBsG + c_sqrt3 * cAsBcG30) + fy[2] * (a_sqrt3 * sAsG - c_sqrt3 * sAcG30) + fz[2] * (-a_sqrt3 *
cAcBsG + c_sqrt3 * cAcBcG30);
fbeta[2] = fx[2] * (a_sqrt3 * (sBcG - sAcBsG) + c_sqrt3 * (sBsG30 + sAcBcG30)) + fz[2] * (a_sqrt3 * (cBcG + sAsBsG) + c_sqrt3 *
(cBsG30 - sAsBcG30));
fgamma[2] = fx[2] * (a_sqrt3 * (cBsG - sAsBcG) - c_sqrt3 * (cBcG30 + sAsBsG30)) + fy[2] * (-a_sqrt3 * cAcG - c_sqrt3 * cAsG30) +
fz[2] * (-a_sqrt3 * (sBsG + sAcBcG) + c_sqrt3 * (sBcG30 - sAcBsG30));
falfa[3] = fx[3] * (-a_sqrt3 * cAsBsG - c_sqrt3 * cAsBsG60) + fy[3] * (a_sqrt3 * sAsG + c_sqrt3 * sAsG60) + fz[3] * (-a_sqrt3 *
cAcBsG - c_sqrt3 * cAcBsG60);
fbeta[3] = fx[3] * (a_sqrt3 * (sBcG - sAcBsG) + c_sqrt3 * (sBcG60 - sAcBsG60)) + fz[3] * (a_sqrt3 * (cBcG + sAsBsG) + c_sqrt3 *
(cBcG60 + sAsBsG60));
fgamma[3] = fx[3] * (a_sqrt3 * (cBsG - sAsBcG) + c_sqrt3 * (cBsG60 - sAsBcG60)) + fy[3] * (-a_sqrt3 * cAcG - c_sqrt3 * cAcG60) +
fz[3] * (-a_sqrt3 * (sBsG + sAcBcG) - c_sqrt3 * (sBsG60 + sAcBcG60));
falfa[4] = fx[4] * (-a_sqrt3 * cAsBcG30 - c_sqrt3 * cAsBsG60) + fy[4] * (a_sqrt3 * sAcG30 + c_sqrt3 * sAsG60) + fz[4] * (-a_sqrt3 *
cAcBcG30 - c_sqrt3 * cAcBsG60);
fbeta[4] = fx[4] * (-a_sqrt3 * (sBsG30 + sAcBcG30) + c_sqrt3 * (sBcG60 - sAcBsG60)) + fz[4] * (a_sqrt3 * (-cBsG30 + sAsBcG30) +
c_sqrt3 * (cBcG60 + sAsBsG60));
fgamma[4] = fx[4] * (a_sqrt3 * (cBcG30 + sAsBsG30) + c_sqrt3 * (cBsG60 - sAsBcG60)) + fy[4] * (a_sqrt3 * cAsG30 - c_sqrt3 *
cAcG60) + fz[4] * (a_sqrt3 * (-sBcG30 + sAcBsG30) - c_sqrt3 * (sBsG60 + sAcBcG60));
falfa[5] = fx[5] * (-a_sqrt3 * cAsBcG30 + c_sqrt3 * cAsBsG) + fy[5] * (a_sqrt3 * sAcG30 - c_sqrt3 * sAsG) + fz[5] * (-a_sqrt3 *
cAcBcG30 + c_sqrt3 * cAcBsG);
fbeta[5] = fx[5] * (-a_sqrt3 * (sBsG30 + sAcBcG30) + c_sqrt3 * (-sBcG + sAcBsG)) + fz[5] * (a_sqrt3 * (-cBsG30 + sAsBcG30) c_sqrt3 * (cBcG + sAsBsG));
fgamma[5] = fx[5] * (a_sqrt3 * (cBcG30 + sAsBsG30) + c_sqrt3 * (-cBsG + sAsBcG)) + fy[5] * (a_sqrt3 * cAsG30 + c_sqrt3 * cAcG)
+ fz[5] *(a_sqrt3 * (-sBcG30 + sAcBsG30) + c_sqrt3 * (sBsG + sAcBcG));
// Resolución del sistema de ecuaciones lineales
double [][] selA =
{
{fx[0], fy[0], fz[0], falfa[0], fbeta[0], fgamma[0]},
{fx[1], fy[1], fz[1], falfa[1], fbeta[1], fgamma[1]},
{fx[2], fy[2], fz[2], falfa[2], fbeta[2], fgamma[2]},
{fx[3], fy[3], fz[3], falfa[3], fbeta[3], fgamma[3]},
{fx[4], fy[4], fz[4], falfa[4], fbeta[4], fgamma[4]},
{fx[5], fy[5], fz[5], falfa[5], fbeta[5], fgamma[5]}
};
double [][] selB = {{-f[0]}, {-f[1]}, {-f[2]}, {-f[3]}, {-f[4]}, {-f[5]}};
double [][] deltas = Matrix.Multiplica (Matrix.inv(selA), selB);
p_x = p_x + deltas[0][0];
p_y = p_y + deltas[1][0];
p_z = p_z + deltas[2][0];
p_alfa = p_alfa + deltas[3][0];
p_beta = p_beta + deltas[4][0];
p_gamma = p_gamma + deltas[5][0];
norma = Math.sqrt (Math.pow(deltas[0][0], 2) + Math.pow(deltas[1][0], 2) + Math.pow(deltas[2][0], 2) + Math.pow(deltas[3][0], 2) +
Math.pow(deltas[4][0], 2) + Math.pow(deltas[5][0], 2));
count_itera++;
}
204
if (count_itera < MAX_ITERA) {
px = trunc(p_x, PRECISION);
py = trunc(p_y, PRECISION);
pz = trunc(p_z, PRECISION);
alfa = trunc(p_alfa, PRECISION);
beta = trunc(p_beta, PRECISION);
gamma = trunc(p_gamma, PRECISION);
calculateTopVertex ();
calculateQVertex ();
calculateLAngles ();
calculateRotState ();
}
else
calculateInverseKinematics ();
}
}
D.4. Clase OpenGLCanvas
import java.awt.*;
import java.awt.event.*;
import gl4java.GLContext;
import gl4java.awt.GLAnimCanvas;
import gl4java.utils.glut.*;
public class OpenGLCanvas extends GLAnimCanvas implements KeyListener, MouseListener {
// Ambiente 3D
boolean [] keys = new boolean [256];
// Holds information on which keys are held down
long quadratic;
// Storage For Our Quadratic Objects
float [] LightAmbient =
{ 0.5f, 0.5f, 0.5f, 1.0f };
float [] LightDiffuse =
{ 1.0f, 1.0f, 1.0f, 1.0f };
float [] LightPosition =
{ 1.0f, 1.0f, 6.0f, 1.0f };
float [] MaterialAmbient =
{ 0.25f, 0.25f, 0.25f, 1.0f };
float [] MaterialDiffuse = { 0.40f, 0.40f, 0.40f, 1.0f };
float [] MaterialSpecular =
{ 0.774597f, 0.774597f, 0.774597f, 1.0f };
float MaterialShininess = 76.8f;
float rEye, tEye, pEye;
boolean showWorkspace = false;
boolean ready = false;
Plataform robot;
Polygonizer pol;
// Constructor
public OpenGLCanvas (int w, int h, Plataform robot, Polygonizer pol) {
super(w, h);
this.robot = robot;
this.pol = pol;
rEye = 20.0f;
tEye = 270.0f;
pEye = 45.0f;
addKeyListener(this);
addMouseListener(this);
setAnimateFps(60);
}
// Métodos necesarios por OpenGL
public void reshape (int width, int height) {
if (height == 0)
height = 1;
gl.glViewport(0, 0, width, height);
gl.glMatrixMode(GL_PROJECTION);
gl.glLoadIdentity();
glu.gluPerspective(45.0f, width / height, 0.1f, 100.0f);
gl.glMatrixMode(GL_MODELVIEW);
gl.glLoadIdentity();
}
public void preInit() {
doubleBuffer = true;
// Reset The Current Viewport And Perspective Transformation
// Select The Projection Matrix
// Reset The Projection Matrix
// Calculate The Aspect Ratio Of The Window
// Select The Modelview Matrix
// Reset The ModalView Matrix
205
stereoView = false;
}
public void init() {
float width = (float) getSize().width;
float height = (float) getSize().height;
gl.glBlendFunc(GL_SRC_ALPHA,GL_ONE);
gl.glShadeModel(GL_SMOOTH);
gl.glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
gl.glClearDepth(1.0);
gl.glEnable(GL_DEPTH_TEST);
gl.glDepthFunc(GL_LEQUAL);
gl.glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
gl.glEnable(GL_LIGHT1);
gl.glLightfv(GL_LIGHT1, GL_AMBIENT, LightAmbient);
gl.glLightfv(GL_LIGHT1, GL_DIFFUSE, LightDiffuse);
gl.glLightfv(GL_LIGHT1, GL_POSITION,LightPosition);
quadratic = glu.gluNewQuadric();
glu.gluQuadricNormals(quadratic, GLU_SMOOTH);
glu.gluQuadricTexture(quadratic, GL_TRUE);
// Set The Blending Function For Translucency
// Enables Smooth Color Shading
// Clear The Background Color To Black
// Enables Clearing Of The Depth Buffer
// Enables Depth Testing
// The Type Of Depth Test To Do
// Really Nice Perspective Calculations
// Enable Light One
// Setup The Ambient Light
// Setup The Diffuse Light
// Position The Light
// Create A Pointer To The Quadric Object
// Create Smooth Normals
// Create Texture Coords
}
// Dibujo de la escena
public void DrawGLScene() {
ready = true;
gl.glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Clear The Screen And The Depth Buffer
gl.glLoadIdentity();
// Reset The View
glu.gluLookAt(rEye * Math.cos(tEye * Math.PI/180) * Math.sin (pEye * Math.PI/180), rEye * Math.sin (tEye * Math.PI/180) * Math.sin (pEye *
Math.PI/180), rEye * Math.cos (pEye * Math.PI/180), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
gl.glColor3f (0.25f, 0.25f, 0.25f);
gl.glBegin (GL_LINES);
gl.glVertex3f (-10.0f, 0.0f, 0.0f);
gl.glVertex3f ( 10.0f, 0.0f, 0.0f);
gl.glVertex3f (0.0f, -10.0f, 0.0f);
gl.glVertex3f (0.0f, 10.0f, 0.0f);
gl.glVertex3f (0.0f, 0.0f, -10.0f);
gl.glVertex3f (0.0f, 0.0f, 10.0f);
gl.glEnd();
gl.glMaterialfv (GL_FRONT_AND_BACK, GL_AMBIENT, MaterialAmbient);
gl.glMaterialfv (GL_FRONT_AND_BACK, GL_DIFFUSE, MaterialDiffuse);
gl.glMaterialfv (GL_FRONT_AND_BACK, GL_SPECULAR, MaterialSpecular);
gl.glMaterialf (GL_FRONT_AND_BACK, GL_SHININESS, MaterialShininess);
gl.glEnable(GL_LIGHTING);
DrawRobot();
if (showWorkspace)
DrawWorkspace();
gl.glDisable(GL_LIGHTING);
}
// Dibujo del robot
public void DrawRobot () {
// Top Variables
float top_espesor = 0.32f;
float top_radio_centro = 0.25f;
float top_radio_borde = 0.16f;
Vector top_normal_up, top_normal_down;
Vector top_normal_height;
Vector [] top_vertex_middle = new Vector [6];
Vector [] top_vertex_front = new Vector [6];
Vector [] top_vertex_back = new Vector [6];
Vector [] top_normal_face = new Vector [6];
// Link P Variables
float link_P_espesor = 0.31f;
float link_P_diametro_conector = 0.58f;
// Link L Variables
float link_L_diametro = 0.22f;
206
// Rotula & Aro Variables
float rotula_largo_eje = 0.217f;
float rotula_diametro_eje = 0.32f;
float rotula_radio = 0.225f;
float aro_espesor = 0.22f;
float aro_radio = 0.225f;
// Auxiliar Variables
double x, y, z;
Vector dir;
float phi, theta;
// Base Lines
gl.glBegin(GL_LINE_LOOP);
for (int j = 6; j > 0; j--)
gl.glVertex3f((float) robot.getBase(j).x, (float) robot.getBase(j).y, (float) robot.getBase(j).z);
gl.glEnd();
// Top Vertex
x = Math.cos (robot.getAlfa (robot.RAD)) * Math.sin (robot.getBeta (robot.RAD));
y = -Math.sin (robot.getAlfa (robot.RAD));
z = Math.sqrt (1 - Math.pow(x, 2) - Math.pow(y, 2));
top_normal_up = new Vector (x, y, z);
top_normal_down = Vector.scalev (-1, top_normal_up);
for (int j = 1; j <= 6; j++) {
top_vertex_front [j - 1] = Vector.sumv (robot.getTop(j), Vector.scalev (top_espesor / 2, top_normal_up));
top_vertex_back [j - 1] = Vector.sumv (robot.getTop(j), Vector.scalev (top_espesor / 2, top_normal_down));
}
for (int j = 1; j <= 6; j++)
top_normal_face [j - 1] = Vector.unitvec (Vector.cross (Vector.diffv (robot.getTop(j % 6 + 1), top_vertex_back[j % 6]), Vector.diffv
(robot.getTop(j), robot.getTop(j % 6 + 1))));
for (int j = 1; j <= 6; j++) {
top_vertex_middle [j - 1] = Vector.sumv (robot.getTop(j), Vector.scalev (-rotula_radio - rotula_largo_eje, top_normal_face [j % 2 == 0? (j
+ 4) % 6 : (j + 5) % 6]));
top_vertex_front [j - 1] = Vector.sumv (top_vertex_middle [j - 1], Vector.scalev (top_espesor / 2, top_normal_up));
top_vertex_back [j - 1] = Vector.sumv (top_vertex_middle [j - 1], Vector.scalev (top_espesor / 2, top_normal_down));
}
// Top Lines
gl.glBegin (GL_LINE_LOOP);
for (int j = 1; j <= 6; j++)
gl.glVertex3f ((float) robot.getTop(j).x, (float) robot.getTop(j).y, (float) robot.getTop(j).z);
gl.glEnd();
// Top Front
gl.glBegin (GL_POLYGON);
gl.glNormal3f ((float) top_normal_up.x, (float) top_normal_up.y, (float) top_normal_up.z);
for (int j = 0; j < 6; j++)
gl.glVertex3f ((float) top_vertex_front[j].x, (float) top_vertex_front[j].y, (float) top_vertex_front[j].z);
gl.glEnd();
// Top Back
gl.glBegin (GL_POLYGON);
gl.glNormal3f ((float) top_normal_down.x, (float) top_normal_down.y, (float) top_normal_down.z);
for (int j = 0; j < 6; j++)
gl.glVertex3f ((float) top_vertex_back[j].x, (float) top_vertex_back[j].y, (float) top_vertex_back[j].z);
gl.glEnd();
// Top Border
for (int j = 6; j > 0; j--) {
dir = Vector.unitvec (Vector.diffv (robot.getTop((j % 6) + 1), robot.getTop(j)));
phi = (float) (180.0 * Math.atan2 (dir.y, dir.x) / Math.PI);
theta = (float) (180.0 * Math.acos (dir.z) / Math.PI);
drawCylinder (top_radio_borde, (float) Vector.norm (Vector.diffv (top_vertex_middle [j % 6], top_vertex_middle [j - 1])), phi, theta, 0.0f,
false, (float) top_vertex_middle [j - 1].x, (float) top_vertex_middle [j - 1].y, (float) top_vertex_middle [j - 1].z);
}
// Top Centroid
drawSphere (top_radio_centro, (float) robot.getPx(), (float) robot.getPy(), (float) robot.getPz());
207
// Arms
for (int j = 6; j > 0; j--) {
// P Line
gl.glBegin(GL_LINES);
gl.glVertex3f((float) robot.getBase(j).x, (float) robot.getBase(j).y, (float) robot.getBase(j).z);
gl.glVertex3f((float) robot.getQ(j).x, (float) robot.getQ(j).y, (float) robot.getQ(j).z);
gl.glEnd();
// Link P Parameters
x = (-3 * rotula_radio / 4 - rotula_largo_eje - link_P_espesor / 2) * robot.getElbowRot(j).nx.x;
y = (-3 * rotula_radio / 4 - rotula_largo_eje - link_P_espesor / 2) * robot.getElbowRot(j).nx.y;
// Revolute Joint
drawDisk (link_P_diametro_conector / 2, link_P_espesor, (float) robot.getSigma (j, robot.DEG), 0.0f, 0.0f, (float) (x + robot.getBase(j).x),
(float) (y + robot.getBase(j).y), (float) robot.getBase(j).z);
// Link P
drawCylinder (link_P_espesor / 2, (float) robot.getP(), (float) (robot.getSigma (j, robot.DEG) - 90.0f), (float) (robot.getTheta (j,
robot.DEG) - 180.0f), 0.0f, false, (float) (x + robot.getBase(j).x), (float) (y + robot.getBase(j).y), (float) robot.getBase(j).z);
// 1st, Spherical Joint Axis Support
drawDisk (link_P_diametro_conector / 2, link_P_espesor, (float) robot.getSigma (j, robot.DEG), 0.0f, 0.0f, (float) (x + robot.getQ(j).x),
(float) (y + robot.getQ(j).y), (float) robot.getQ(j).z);
// 1st. Spherical Joint Axis
x = (-3 * rotula_radio / 4 - rotula_largo_eje) * robot.getElbowRot(j).nx.x;
y = (-3 * rotula_radio / 4 - rotula_largo_eje) * robot.getElbowRot(j).nx.y;
drawCylinder (rotula_diametro_eje / 2, rotula_largo_eje, (float) robot.getSigma (j, robot.DEG), 90.0f, 0.0f, false, (float) (x +
robot.getQ(j).x), (float) (y + robot.getQ(j).y), (float) robot.getQ(j).z);
drawCylinder (rotula_diametro_eje / 2, 2 * rotula_radio, (float) robot.getSigma (j, robot.DEG), 90.0f, 0.0f, true, (float) robot.getQ(j).x,
(float) robot.getQ(j).y, (float) robot.getQ(j).z);
// 1st. Spherical Joint
drawSphere (rotula_radio, (float) robot.getQ(j).x, (float) robot.getQ(j).y, (float) robot.getQ(j).z);
drawDisk (aro_radio, aro_espesor, (float) robot.getPhiL (j, robot.DEG), (float) robot.getThetaL (j, robot.DEG), (float) robot.getFiL (j,
robot.DEG), (float) robot.getQ(j).x, (float) robot.getQ(j).y, (float) robot.getQ(j).z);
// Link L
drawCylinder (link_L_diametro / 2, (float) robot.getL(), (float) robot.getPhiL (j, robot.DEG), (float) robot.getThetaL (j, robot.DEG), 0.0f,
false, (float) robot.getQ(j).x, (float) robot.getQ(j).y, (float) robot.getQ(j).z);
// 2nd. Spherical Joint Axis
x = (3 * rotula_radio / 4) * robot.getTopRot(j).nx.x;
y = (3 * rotula_radio / 4) * robot.getTopRot(j).nx.y;
z = (3 * rotula_radio / 4) * robot.getTopRot(j).nx.z;
dir = robot.getTopRot(j).nx;
phi = (float) (180.0 * Math.atan2 (dir.y, dir.x) / Math.PI);
theta = (float) (180.0 * Math.acos (dir.z) / Math.PI);
drawCylinder (rotula_diametro_eje / 2, rotula_largo_eje, phi, theta, 0.0f, false, (float) (x + robot.getTop(j).x), (float) (y +
robot.getTop(j).y), (float) (z + robot.getTop(j).z));
drawCylinder (rotula_diametro_eje / 2, 2 * rotula_radio, phi, theta, 0.0f, true, (float) robot.getTop(j).x, (float) robot.getTop(j).y, (float)
robot.getTop(j).z);
// 2nd. Spherical Joint
drawSphere (rotula_radio, (float) robot.getTop(j).x, (float) robot.getTop(j).y, (float) robot.getTop(j).z);
drawDisk (aro_radio, aro_espesor, (float) robot.getPhiL (j, robot.DEG), (float) robot.getThetaL (j, robot.DEG), (float) robot.getFiL (j,
robot.DEG), (float) robot.getTop(j).x, (float) robot.getTop(j).y, (float) robot.getTop(j).z);
}
}
// Dibujo del espacio de trabajo
public void DrawWorkspace () {
gl.glEnable(GL_BLEND);
for (int j = 0; j < pol.getPolygons().size(); j++) {
Polygon tri = (Polygon) pol.getPolygons().get(j);
208
gl.glBegin(GL_TRIANGLES);
for (int k = 0; k < 3; k++) {
gl.glNormal3f ((float)tri.n[k].x, (float)tri.n[k].y, (float)tri.n[k].z);
gl.glVertex3f ((float)tri.p[k].x, (float)tri.p[k].y, (float)tri.p[k].z);
}
gl.glEnd ();
}
gl.glDisable(GL_BLEND);
}
public void keyPressed (KeyEvent e) {
// Dibujo de elementos simples
switch (e.getKeyCode()) {
public void drawSphere (float rad, float center_x, float center_y,
float center_z) {
case KeyEvent.VK_ESCAPE:
System.exit(0);
break;
gl.glPushMatrix();
gl.glTranslatef (center_x, center_y, center_z);
glu.gluSphere (quadratic, rad, 32, 32);
gl.glPopMatrix();
default :
if (e.getKeyCode() < 250)
keys [e.getKeyCode()] = true;
break;
}
public void drawCylinder (float rad, float length, float phi_cyl, float
theta_cyl, float fi_cyl, boolean from_middle, float center_x, float center_y,
float center_z) {
}
// Navigation
gl.glPushMatrix();
gl.glTranslatef (center_x, center_y, center_z);
gl.glRotatef (phi_cyl, 0.0f, 0.0f, 1.0f);
gl.glRotatef (theta_cyl, 0.0f, 1.0f, 0.0f);
gl.glRotatef (fi_cyl, 0.0f, 0.0f, 1.0f);
if (keys[KeyEvent.VK_UP])
rEye -= 0.5;
if (keys[KeyEvent.VK_DOWN])
rEye += 0.5;
if (keys[KeyEvent.VK_RIGHT])
tEye += 0.2;
if (keys[KeyEvent.VK_LEFT])
tEye -= 0.2;
if (keys[KeyEvent.VK_PAGE_UP])
pEye -= 0.2;
if (keys[KeyEvent.VK_PAGE_DOWN])
pEye += 0.2;
if (from_middle)
gl.glTranslatef (0.0f, 0.0f, -length / 2.0f);
glu.gluCylinder (quadratic, rad, rad, length, 32, 32);
glu.gluDisk (quadratic, 0.0f, rad, 32, 32);
gl.glTranslatef (0.0f, 0.0f, length);
glu.gluDisk (quadratic, 0.0f, rad, 32, 32);
gl.glPopMatrix();
// Workspace Display
}
if (keys['W']) {
showWorkspace = !showWorkspace;
pol.start ();
}
public void drawDisk (float rad, float length, float phi_cyl, float
theta_cyl, float fi_cyl, float center_x, float center_y, float center_z) {
gl.glPushMatrix();
gl.glTranslatef (center_x, center_y, center_z);
gl.glRotatef (phi_cyl, 0.0f, 0.0f, 1.0f);
gl.glRotatef (theta_cyl, 0.0f, 1.0f, 0.0f);
gl.glRotatef (fi_cyl, 0.0f, 0.0f, 1.0f);
display();
}
public void keyReleased (KeyEvent e) {
if (e.getKeyCode() < 250)
keys [e.getKeyCode()] = false;
}
gl.glRotatef (90.0f, 0.0f, 1.0f, 0.0f);
gl.glTranslatef (0.0f, 0.0f, -length / 2.0f);
glu.gluCylinder (quadratic, rad, rad, length, 32, 32);
glu.gluDisk (quadratic, 0.0f, rad, 32, 32);
gl.glTranslatef (0.0f, 0.0f, length);
glu.gluDisk (quadratic, 0.0f, rad, 32, 32);
gl.glPopMatrix();
}
public void mouseEntered (MouseEvent evt) {
Component comp = evt.getComponent();
if (comp.equals(this))
requestFocus();
}
// Control de la escena
public void mouseExited (MouseEvent evt) {}
public void display() {
glj.gljMakeCurrent();
DrawGLScene();
glj.gljSwap();
glj.gljFree();
}
public void mousePressed (MouseEvent evt) {}
// Ensure GL is initialised correctly
public void mouseReleased (MouseEvent evt) {}
// Swap buffers
// Release GL
public void mouseClicked (MouseEvent evt) {
Component comp = evt.getComponent();
if (comp.equals(this))
requestFocus();
}
public void keyTyped (KeyEvent e) {}
}
209
D.5. Clase Polygonizer
else
import java.util.LinkedList;
if (v3.fval < 0)
tri (v2, v0, v1, v3);
else
quad (v2, v3, v0, v1);
public class Polygonizer {
private final static int MAX_REC = 3;
private final static double STEP_0 = 0.125;
private static double eps_dist = 0.00001;
private static double dot_tol = 0.5;
private static Vector ll = new Vector (-3, -3, 0);
private static Vector ur = new Vector (3, 3, 8);
private static double nx = 8, ny = 8, nz = 8;
private LinkedList polygons;
private Implicit function;
else
if (v2.fval < 0)
if (v3.fval < 0)
tri (v1, v3, v2, v0);
else
quad (v1, v3, v2, v0);
else
if (v3.fval < 0)
quad (v1, v2, v0, v3);
else
tri (v0, v3, v2, v1);
class VertexPol {
Vector p = new Vector ();
Vector n = new Vector ();
double fval;
}
else
if (v1.fval < 0)
if (v2.fval < 0)
if (v3.fval < 0)
tri (v0, v1, v2, v3);
else
quad (v0, v3, v1, v2);
else
if (v3.fval < 0)
quad (v0, v2, v3, v1);
else
tri (v1, v3, v0, v2);
else
if (v2.fval < 0)
if (v3.fval < 0)
quad (v0, v1, v2, v3);
else
tri (v2, v3, v1, v0);
else
if (v3.fval < 0)
tri(v3, v0, v1, v2);
else
;
public Polygonizer (Implicit f) {
function = f;
}
public LinkedList getPolygons () {
return polygons;
}
public void start () {
polygons = new LinkedList ();
vol_scan (ll.x, ll.y, ll.z, ur.x, ur.y, ur.z,(ur.x - ll.x) / nx, (ur.y - ll.y)
/ ny, (ur.z - ll.z) / nz);
}
void vol_scan (double x0, double y0, double z0, double x1, double
y1, double z1, double xinc, double yinc, double zinc) {
VertexPol [] v = new VertexPol [8];
int k, side = 0;
boolean hit;
double x, y, z;
}
void tri (VertexPol v0, VertexPol v1, VertexPol v2, VertexPol v3) {
for (z = z0; z <= z1; z += zinc)
for (y = y0; y <= y1; y += yinc)
for (x = x0; x <= x1; x += xinc) {
for (k = 0, hit = false; k < 8; k++) {
Vector i0, i1, i2;
Vector n0, n1, n2;
i0 = intersect (v0, v1);
i1 = intersect (v0, v2);
i2 = intersect (v0, v3);
n0 = function.f_ngrad (i0);
n1 = function.f_ngrad (i1);
n2 = function.f_ngrad (i2);
adapt_tri (i0, i1, i2, n0, n1, n2, MAX_REC);
v[k] = new VertexPol ();
v[k].p.x = (k & 1) > 0 ? x : x - xinc;
v[k].p.y = (k & 2) > 0 ? y : y - yinc;
v[k].p.z = (k & 4) > 0 ? z : z - zinc;
v[k].fval = function.f_value (v[k].p);
}
if (k == 0)
side = sign (v[0].fval);
else if (side != sign (v[k].fval))
hit = true;
void quad (VertexPol v0, VertexPol v1, VertexPol v2, VertexPol v3) {
Vector i0, i1, i2, i3;
Vector n0, n1, n2, n3;
i0 = intersect (v0, v2);
i1 = intersect (v0, v3);
i2 = intersect (v1, v3);
i3 = intersect (v1, v2);
n0 = function.f_ngrad (i0);
n1 = function.f_ngrad (i1);
n2 = function.f_ngrad (i2);
n3 = function.f_ngrad (i3);
adapt_tri (i0, i1, i2, n0, n1, n2, MAX_REC);
adapt_tri (i0, i2, i3, n0, n2, n3, MAX_REC);
}
if (hit) {
simplex (v[0], v[1], v[3], v[7]);
simplex (v[0], v[5], v[1], v[7]);
simplex (v[0], v[3], v[2], v[7]);
simplex (v[0], v[2], v[6], v[7]);
simplex (v[0], v[4], v[5], v[7]);
simplex (v[0], v[6], v[4], v[7]);
}
}
}
}
void simplex (VertexPol v0, VertexPol v1, VertexPol v2, VertexPol
Vector intersect (VertexPol v0, VertexPol v1) {
v3) {
if (v0.fval < 0)
if (v1.fval < 0)
if (v2.fval < 0)
if (v3.fval < 0)
;
else
tri (v3, v2, v1, v0);
double t;
Vector p;
t = v0.fval / (v0.fval - v1.fval);
p = Vector.lerpvec (v0.p, v1.p, t);
return project (STEP_0, p, function.f_value(p), MAX_REC);
}
210
boolean edge_code (Vector n0, Vector n1) {
void output_tri (Vector p0, Vector p1, Vector p2, Vector n0, Vector
n1, Vector n2) {
if (Vector.dot (Vector.cross (Vector.diffv(p1, p0),
Vector.diffv(p2, p0)), n0) < 0) {
Vector px = new Vector (p1.x, p1.y, p1.z);
Vector nx = new Vector (n1.x, n1.y, n1.z);
p1 = new Vector (p2.x, p2.y, p2.z);
n1 = new Vector (n2.x, n2.y, n2.z);
p2 = new Vector (px.x, px.y, px.z);
n2 = new Vector (nx.x, nx.y, nx.z);
}
polygons.add (new Polygon (p0, p1, p2, n0, n1, n2));
if (Vector.dot (n0, n1) < dot_tol)
return false;
else
return true;
}
Vector midpoint (boolean e, Vector p0, Vector p1) {
Vector pm = Vector.scalev (0.5, Vector.sumv(p0, p1));
if (e == false)
pm = project (STEP_0, pm, function.f_value(pm),
MAX_REC);
}
return pm;
}
void adapt_tri (Vector p0, Vector p1, Vector p2, Vector n0, Vector
n1, Vector n2, int k) {
Vector project (double s, Vector p, double v0, int k) {
boolean e1, e2, e3;
Vector pm1, pm2, pm3;
Vector nm1, nm2, nm3;
e1 = edge_code (n0, n1);
e2 = edge_code (n1, n2);
e3 = edge_code (n2, n0);
if ((e1 && e2 && e3) || k-- == 0)
output_tri (p0, p1, p2, n0, n1, n2);
else {
pm1 = midpoint (e1, p0, p1);
pm2 = midpoint (e2, p1, p2);
pm3 = midpoint (e3, p2, p0);
nm1 = function.f_ngrad (pm1);
nm2 = function.f_ngrad (pm2);
nm3 = function.f_ngrad (pm3);
adapt_tri (p0, pm1, pm3, n0, nm1, nm3, k);
adapt_tri (p1, pm2, pm1, n1, nm2, nm1, k);
adapt_tri (p2, pm3, pm2, n2, nm3, nm2, k);
adapt_tri (pm1, pm2, pm3, nm1, nm2, nm3, k);
}
double v1;
p = Vector.sumv (p, Vector.scalev (s * sign (v0) * -1.0,
function.f_ngrad (p)));
v1 = function.f_value (p);
if ((Math.abs (v1) < eps_dist) || k-- == 0)
return p;
if (v0 * v1 < 0.0)
s /= 2;
return project (s, p, v1, k);
}
int sign (double v) {
return (v < 0.0)? -1 : 1;
}
}
}
D.6. Clase Controls
import java.awt.*;
import java.awt.event.*;
import java.io.IOException;
import javax.swing.*;
import javax.swing.event.*;
import javax.swing.border.*;
import javax.swing.plaf.basic.*;
import java.util.*;
import com.centralnexus.input.*;
JPanel jpanLblOri;
JPanel jpanTxtOri;
JPanel jpanBtnOri;
JPanel jpanUDaOri;
JPanel jpanUDbOri;
JPanel jpanUDgOri;
JPanel jpanInvKinOri;
class Controls extends JPanel implements ActionListener,
JoystickListener, Runnable {
JPanel jpanActuatorsControls;
JPanel jpanLblAct;
JPanel jpanTxtAct;
JPanel jpanBtnOptAct;
JPanel [] jpanEachOptAct = new JPanel [6];
JPanel [] jpanUDAct = new JPanel [6];
JPanel [] jpanEachBtnOptAct = new JPanel [6];
JPanel jpanDirKinAct;
// Declaración de contenedores
JPanel jpanInvKinControls;
JPanel jpanDirKinControls;
// Para los actuadores
// Para la posición
// Declaración de componentes
JPanel jpanPositionControls;
JPanel jpanLblPos;
JPanel jpanTxtPos;
JPanel jpanBtnPos;
JPanel jpanUDxPos;
JPanel jpanUDyPos;
JPanel jpanUDzPos;
JPanel jpanInvKinPos;
// Para la orientación
JPanel jpanOrientationControls;
// Para la posición
JLabel jlblPx;
JLabel jlblPy;
JLabel jlblPz;
JTextField jtxtPx;
JTextField jtxtPy;
JTextField jtxtPz;
BasicArrowButton jbtnPxDown;
BasicArrowButton jbtnPxUp;
BasicArrowButton jbtnPyDown;
211
BasicArrowButton jbtnPyUp;
BasicArrowButton jbtnPzDown;
BasicArrowButton jbtnPzUp;
JButton jbtnInvKinPos;
// Para la orientación
JLabel jlblAlfa;
JLabel jlblBeta;
JLabel jlblGamma;
JTextField jtxtAlfa;
JTextField jtxtBeta;
JTextField jtxtGamma;
BasicArrowButton jbtnAlfaDown;
BasicArrowButton jbtnAlfaUp;
BasicArrowButton jbtnBetaDown;
BasicArrowButton jbtnBetaUp;
BasicArrowButton jbtnGammaDown;
BasicArrowButton jbtnGammaUp;
JButton jbtnInvKinOri;
// Para los actuadores
JLabel [] jlblTheta = new JLabel [6];
JTextField [] jtxtTheta = new JTextField [6];
JRadioButton [] jrbtTheta1 = new JRadioButton [6];
JRadioButton [] jrbtTheta2 = new JRadioButton [6];
ButtonGroup [] jbgpTheta = new ButtonGroup [6];
BasicArrowButton [] jbtnThetaDown = new BasicArrowButton [6];
BasicArrowButton [] jbtnThetaUp = new BasicArrowButton [6];
JButton jbtnDirKinAct;
// Declaración de referencias
Plataform parallelRobot;
Polygonizer pol;
OpenGLCanvas opengl_canvas;
// Declaración para el joystick
Joystick joy;
boolean translation = true;
boolean orientation = false;
boolean active_repos = true;
boolean active_reori = true;
boolean joystick_run = false;
int pressed = 0;
int [] pov = new int [2];
double joyX, joyY, joyZ, joyR;
double joyXold, joyYold, joyZold, joyRold;
int theta_chosen = 1;
int but;
Thread thread = new Thread(this);
// Constructor
public Controls (Plataform parallelRobot, Polygonizer pol, OpenGLCanvas opengl_canvas) throws IOException {
this.parallelRobot = parallelRobot;
this.pol = pol;
this.opengl_canvas = opengl_canvas;
joy = Joystick.createInstance();
// Inicialización de Controles
// Para la posición
jlblPx = new JLabel ("Sobre X: ");
jlblPy = new JLabel ("Sobre Y: ");
jlblPz = new JLabel ("Sobre Z: ");
jtxtPx = new JTextField (String.valueOf (trunc (parallelRobot.getPx(), 4)));
jtxtPy = new JTextField (String.valueOf (trunc (parallelRobot.getPy(), 4)));
jtxtPz = new JTextField (String.valueOf (trunc (parallelRobot.getPz(), 4)));
jbtnPxDown = new BasicArrowButton (BasicArrowButton.SOUTH);
jbtnPxUp = new BasicArrowButton (BasicArrowButton.NORTH);
jbtnPyDown = new BasicArrowButton (BasicArrowButton.SOUTH);
jbtnPyUp = new BasicArrowButton (BasicArrowButton.NORTH);
jbtnPzDown = new BasicArrowButton (BasicArrowButton.SOUTH);
jbtnPzUp = new BasicArrowButton (BasicArrowButton.NORTH);
jbtnInvKinPos = new JButton ("Ingresar");
// Para la orientación
jlblAlfa = new JLabel ("En Alfa (°): ");
jlblBeta = new JLabel ("En Beta (°): ");
jlblGamma = new JLabel ("En Gamma (°): ");
jtxtAlfa = new JTextField (String.valueOf (trunc (parallelRobot.getAlfa(Plataform.DEG), 4)));
jtxtBeta = new JTextField (String.valueOf (trunc (parallelRobot.getBeta(Plataform.DEG), 4)));
jtxtGamma = new JTextField (String.valueOf (trunc (parallelRobot.getGamma(Plataform.DEG), 4)));
jbtnAlfaDown = new BasicArrowButton (BasicArrowButton.SOUTH);
jbtnAlfaUp = new BasicArrowButton (BasicArrowButton.NORTH);
jbtnBetaDown = new BasicArrowButton (BasicArrowButton.SOUTH);
jbtnBetaUp = new BasicArrowButton (BasicArrowButton.NORTH);
jbtnGammaDown = new BasicArrowButton (BasicArrowButton.SOUTH);
jbtnGammaUp = new BasicArrowButton (BasicArrowButton.NORTH);
jbtnInvKinOri = new JButton ("Ingresar");
// Para los actuadores
for (int j = 0; j < 6; j++) {
jlblTheta [j] = new JLabel ("Theta " + (j + 1) + " (°): ");
jtxtTheta [j] = new JTextField (String.valueOf (trunc (parallelRobot.getTheta(j + 1, Plataform.DEG), 4)));
jrbtTheta1 [j] = new JRadioButton ("1", (j % 2 == 0)? false:true);
jrbtTheta2 [j] = new JRadioButton ("2", (j % 2 == 0)? true:false);
parallelRobot.setThetaOption (j + 1, (j % 2 == 0)? 2:1);
jbgpTheta [j] = new ButtonGroup ();
jbgpTheta [j].add (jrbtTheta1 [j]);
jbgpTheta [j].add (jrbtTheta2 [j]);
jbtnThetaDown [j] = new BasicArrowButton (BasicArrowButton.SOUTH);
212
jbtnThetaUp [j] = new BasicArrowButton (BasicArrowButton.NORTH);
}
jbtnDirKinAct = new JButton ("Ingresar");
// Inicialización de Contenedores
jpanInvKinControls = new JPanel (new GridLayout (0,2));
// Para la posición
jpanPositionControls = new JPanel () {
Insets insets = new Insets(30,20,15,15);
public Insets getInsets() {
return insets;
}
};
jpanLblPos = new JPanel(new GridLayout (3,0, 10, 10));
jpanTxtPos = new JPanel(new GridLayout (3,0, 10, 10));
jpanBtnPos = new JPanel(new GridLayout (3,0, 10, 10));
jpanUDxPos = new JPanel(new FlowLayout (FlowLayout.RIGHT, 0, 0));
jpanUDyPos = new JPanel(new FlowLayout (FlowLayout.RIGHT, 0, 0));
jpanUDzPos = new JPanel(new FlowLayout (FlowLayout.RIGHT, 0, 0));
jpanInvKinPos = new JPanel(new FlowLayout (FlowLayout.RIGHT));
// Para la orientación
jpanOrientationControls = new JPanel () {
Insets insets = new Insets(30,20,15,15);
public Insets getInsets() {
return insets;
}
};
jpanLblOri = new JPanel(new GridLayout (3,0, 10, 10));
jpanTxtOri = new JPanel(new GridLayout (3,0, 10, 10));
jpanBtnOri = new JPanel(new GridLayout (3,0, 10, 10));
jpanUDaOri = new JPanel(new FlowLayout (FlowLayout.RIGHT, 0, 0));
jpanUDbOri = new JPanel(new FlowLayout (FlowLayout.RIGHT, 0, 0));
jpanUDgOri = new JPanel(new FlowLayout (FlowLayout.RIGHT, 0, 0));
jpanInvKinOri = new JPanel(new FlowLayout (FlowLayout.RIGHT));
// Para los actuadores
jpanActuatorsControls = new JPanel () {
Insets insets = new Insets(30,20,15,15);
public Insets getInsets() {
return insets;
}
};
jpanLblAct = new JPanel(new GridLayout (6,0, 10, 10));
jpanTxtAct = new JPanel(new GridLayout (6,0, 10, 10));
jpanBtnOptAct = new JPanel (new GridLayout (6,0, 10, 10));
for (int j = 0; j < 6; j++) {
jpanUDAct [j] = new JPanel(new FlowLayout (FlowLayout.RIGHT, 0, 0));
jpanEachOptAct [j] = new JPanel(new FlowLayout (FlowLayout.CENTER, 0, 0));
jpanEachBtnOptAct [j] = new JPanel (new FlowLayout (FlowLayout.CENTER, 10, 0));
}
jpanDirKinAct = new JPanel(new FlowLayout (FlowLayout.RIGHT));
// Organización de Controles
setLayout (new BorderLayout ());
// Para la posición
jpanPositionControls.setLayout (new BorderLayout(10, 10));
jpanPositionControls.setBorder(new TitledBorder("Posicion del Centro de la Plataforma"));
jpanLblPos.add (jlblPx);
jpanLblPos.add (jlblPy);
jpanLblPos.add (jlblPz);
jpanTxtPos.add (jtxtPx);
jpanTxtPos.add (jtxtPy);
jpanTxtPos.add (jtxtPz);
jpanUDxPos.add (jbtnPxDown);
jpanUDxPos.add (jbtnPxUp);
jpanBtnPos.add (jpanUDxPos);
jpanUDyPos.add (jbtnPyDown);
jpanUDyPos.add (jbtnPyUp);
jpanBtnPos.add (jpanUDyPos);
213
jpanUDzPos.add (jbtnPzDown);
jpanUDzPos.add (jbtnPzUp);
jpanBtnPos.add (jpanUDzPos);
jpanInvKinPos.add (jbtnInvKinPos);
jpanPositionControls.add (jpanLblPos, BorderLayout.WEST);
jpanPositionControls.add (jpanTxtPos, BorderLayout.CENTER);
jpanPositionControls.add (jpanBtnPos, BorderLayout.EAST);
jpanPositionControls.add (jpanInvKinPos, BorderLayout.SOUTH);
add (jpanPositionControls);
// Para la orientación
jpanOrientationControls.setLayout (new BorderLayout(10, 10));
jpanOrientationControls.setBorder(new TitledBorder("Orientación de la Plataforma"));
jpanLblOri.add (jlblAlfa);
jpanLblOri.add (jlblBeta);
jpanLblOri.add (jlblGamma);
jpanTxtOri.add (jtxtAlfa);
jpanTxtOri.add (jtxtBeta);
jpanTxtOri.add (jtxtGamma);
jpanUDaOri.add (jbtnAlfaDown);
jpanUDaOri.add (jbtnAlfaUp);
jpanBtnOri.add (jpanUDaOri);
jpanUDbOri.add (jbtnBetaDown);
jpanUDbOri.add (jbtnBetaUp);
jpanBtnOri.add (jpanUDbOri);
jpanUDgOri.add (jbtnGammaDown);
jpanUDgOri.add (jbtnGammaUp);
jpanBtnOri.add (jpanUDgOri);
jpanInvKinOri.add (jbtnInvKinOri);
jpanOrientationControls.add (jpanLblOri, BorderLayout.WEST);
jpanOrientationControls.add (jpanTxtOri, BorderLayout.CENTER);
jpanOrientationControls.add (jpanBtnOri, BorderLayout.EAST);
jpanOrientationControls.add (jpanInvKinOri, BorderLayout.SOUTH);
add (jpanOrientationControls);
// Para los actuadores
jpanActuatorsControls.setLayout (new BorderLayout(10, 10));
jpanActuatorsControls.setBorder(new TitledBorder("Posición Angular de los Actuadores"));
for (int j = 0; j < 6; j++) {
jpanLblAct.add (jlblTheta [j]);
jpanTxtAct.add (jtxtTheta [j]);
jpanUDAct [j].add (jbtnThetaDown [j]);
jpanUDAct [j].add (jbtnThetaUp [j]);
jpanEachBtnOptAct [j].add (jpanUDAct [j]);
jpanEachOptAct [j].add (jrbtTheta1 [j]);
jpanEachOptAct [j].add (jrbtTheta2 [j]);
jpanEachBtnOptAct [j].add (jpanEachOptAct [j]);
jpanBtnOptAct.add (jpanEachBtnOptAct [j]);
}
jpanDirKinAct.add (jbtnDirKinAct);
jpanActuatorsControls.add (jpanLblAct, BorderLayout.WEST);
jpanActuatorsControls.add (jpanTxtAct, BorderLayout.CENTER);
jpanActuatorsControls.add (jpanBtnOptAct, BorderLayout.EAST);
jpanActuatorsControls.add (jpanDirKinAct, BorderLayout.SOUTH);
add (jpanActuatorsControls);
// Para todos
jpanInvKinControls.add (jpanPositionControls);
jpanInvKinControls.add (jpanOrientationControls);
add (jpanInvKinControls, BorderLayout.NORTH);
add (jpanActuatorsControls, BorderLayout.SOUTH);
// Declaración de listeners
// Para la posición
jtxtPx.addActionListener (this);
jtxtPy.addActionListener (this);
jtxtPz.addActionListener (this);
jbtnPxDown.addActionListener (this);
jbtnPxUp.addActionListener (this);
jbtnPyDown.addActionListener (this);
jbtnPyUp.addActionListener (this);
jbtnPzDown.addActionListener (this);
jbtnPzUp.addActionListener (this);
jbtnInvKinPos.addActionListener (this);
// Para la orientación
214
jtxtAlfa.addActionListener (this);
jtxtBeta.addActionListener (this);
jtxtGamma.addActionListener (this);
jbtnAlfaDown.addActionListener (this);
jbtnAlfaUp.addActionListener (this);
jbtnBetaDown.addActionListener (this);
jbtnBetaUp.addActionListener (this);
jbtnGammaDown.addActionListener (this);
jbtnGammaUp.addActionListener (this);
jbtnInvKinOri.addActionListener (this);
// Para los actuadores
for (int j = 0; j < 6; j++) {
jtxtTheta [j].addActionListener (this);
jbtnThetaDown [j].addActionListener (this);
jbtnThetaUp [j].addActionListener (this);
jrbtTheta1 [j].addActionListener (this);
jrbtTheta2 [j].addActionListener (this);
}
jbtnDirKinAct.addActionListener (this);
// Para el joystick
joy.addJoystickListener(this);
thread.start();
}
// Métodos requeridos por los eventos
public void actionPerformed (ActionEvent ae) {
Object object = ae.getSource();
// Para la posición
if (object.equals (jtxtPx)) {
try {
parallelRobot.setPx (Double.parseDouble(jtxtPx.getText()));
updateBoxes ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jtxtPy)) {
try {
parallelRobot.setPy (Double.parseDouble(jtxtPy.getText()));
updateBoxes ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jtxtPz)) {
try {
parallelRobot.setPz (Double.parseDouble(jtxtPz.getText()));
updateBoxes ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnPxDown)) {
try {
parallelRobot.setPx (parallelRobot.getPx() – 0.1);
updateBoxes ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnPxUp)) {
try {
parallelRobot.setPx (parallelRobot.getPx() + 0.1);
updateBoxes ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnPyDown)) {
try {
parallelRobot.setPy (parallelRobot.getPy() – 0.1);
updateBoxes ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnPyUp)) {
try {
parallelRobot.setPy (parallelRobot.getPy() + 0.1);
updateBoxes ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnPzDown)) {
try {
parallelRobot.setPx (parallelRobot.getPz() – 0.1);
updateBoxes ();
} catch (NumberFormatException e) {}
215
}
else if (object.equals (jbtnPzUp)) {
try {
parallelRobot.setPz (parallelRobot.getPz() + 0.1);
updateBoxes ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnInvKinPos)) {
try {
parallelRobot.setPosition ( Double.parseDouble(jtxtPx.getText()),
Double.parseDouble(jtxtPy.getText()),
Double.parseDouble(jtxtPz.getText()));
updateBoxes ();
} catch (NumberFormatException e) {}
}
// Para la orientación
if (object.equals (jtxtAlfa)) {
try {
parallelRobot.setAlfa (Double.parseDouble(jtxtAlfa.getText()), Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jtxtBeta)) {
try {
parallelRobot.setBeta (Double.parseDouble(jtxtBeta.getText()), Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jtxtGamma)) {
try {
parallelRobot.setGamma (Double.parseDouble(jtxtGamma.getText()), Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnAlfaDown)) {
try {
parallelRobot.setAlfa (parallelRobot.getAlfa(Plataform.DEG) - 1, Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnAlfaUp)) {
try {
parallelRobot.setAlfa (parallelRobot.getAlfa(Plataform.DEG) + 1, Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnBetaDown)) {
try {
parallelRobot.setBeta (parallelRobot.getBeta(Plataform.DEG) - 1, Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnBetaUp)) {
try {
parallelRobot.setBeta (parallelRobot.getBeta(Plataform.DEG) + 1, Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnGammaDown)) {
try {
parallelRobot.setGamma (parallelRobot.getGamma(Plataform.DEG) - 1, Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnGammaUp)) {
try {
216
parallelRobot.setGamma (parallelRobot.getGamma(Plataform.DEG) + 1, Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnInvKinOri)) {
try {
parallelRobot.setOrientation ( Double.parseDouble(jtxtAlfa.getText()),
Double.parseDouble(jtxtBeta.getText()),
Double.parseDouble(jtxtGamma.getText()),
Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
} catch (NumberFormatException e) {}
}
// Para los actuadores
if (object.equals (jbtnDirKinAct)) {
try {
parallelRobot.setTheta ( Double.parseDouble(jtxtTheta[0].getText()),
Double.parseDouble(jtxtTheta[1].getText()),
Double.parseDouble(jtxtTheta[2].getText()),
Double.parseDouble(jtxtTheta[3].getText()),
Double.parseDouble(jtxtTheta[4].getText()),
Double.parseDouble(jtxtTheta[5].getText()),
Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
} catch (NumberFormatException e) {}
}
else
for (int j = 0; j < 6; j++) {
if (object.equals (jtxtTheta [j])) {
try {
parallelRobot.setTheta (Double.parseDouble(jtxtTheta[j].getText()), j + 1, Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
break;
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnThetaDown [j])) {
try {
parallelRobot.setTheta (parallelRobot.getTheta(j + 1, Plataform.DEG) - 1, j + 1, Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
break;
} catch (NumberFormatException e) {}
}
else if (object.equals (jbtnThetaUp [j])) {
try {
parallelRobot.setTheta (parallelRobot.getTheta(j + 1, Plataform.DEG) + 1, j + 1, Plataform.DEG);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
break;
} catch (NumberFormatException e) {}
}
else if (object.equals (jrbtTheta1 [j]) || object.equals (jrbtTheta2 [j])) {
if (getSelection(jbgpTheta [j]).equals (jrbtTheta1 [j]))
parallelRobot.setThetaOption (j + 1, 1);
else
parallelRobot.setThetaOption (j + 1, 2);
updateBoxes ();
if (opengl_canvas.showWorkspace)
pol.start ();
break;
}
}
}
public void joystickAxisChanged (Joystick j) {
joystick_run = false;
double pov_temp = j.getPOV();
if (trunc(pov_temp, 3) * 100 != -1.0) {
217
case 8:
// Cuadrado
translation = true;
orientation = false;
break;
case 64:
// L1
theta_chosen -= 1;
if (theta_chosen < 1)
theta_chosen = 6;
break;
case 128:
// R1
theta_chosen += 1;
if (theta_chosen > 6)
theta_chosen = 1;
break;
case 256:
// Select
opengl_canvas.showWorkspace =
!opengl_canvas.showWorkspace;
if (opengl_canvas.showWorkspace)
pol.start ();
break;
case 512:
// Start
parallelRobot.setTheta (0, 0, 0, 0, 0, 0,
Plataform.DEG);
if (opengl_canvas.showWorkspace)
pol.start ();
break;
case 1024:
// XY
active_repos = !active_repos;
break;
case 2048:
// RZ
active_reori = !active_reori;
break;
}
joystick_run = true;
switch ((int) pov_temp) {
case 0:
pov[0] = 0;
pov[1] = 1;
break;
case 45:
pov[0] = 1;
pov[1] = 1;
break;
case 90:
pov[0] = 1;
pov[1] = 0;
break;
case 135:
pov[0] = 1;
pov[1] = -1;
case 180:
pov[0] = 0;
pov[1] = -1;
break;
case 225:
pov[0] = -1;
pov[1] = -1;
case 270:
pov[0] = -1;
pov[1] = 0;
break;
case 315:
pov[0] = -1;
pov[1] = 1;
break;
}
synchronized (this) {
if (joystick_run) {
pressed = 0;
notify();
}
}
}
else {
pov[0] = 0;
pov[1] = 0;
}
joyXold = joyX;
joyYold = joyY;
joyZold = joyZ;
joyRold = joyR;
joyX = trunc(j.getX() * Math.abs(j.getX()), 4);
joyY = trunc(-j.getY() * Math.abs(j.getY()), 4);
joyZ = trunc(-j.getZ() * Math.abs(j.getZ()), 4);
joyR = trunc(j.getR() * Math.abs(j.getR()), 4);
if ((joyX != 0) || (joyY != 0) || (joyZ != 0) || (joyR != 0))
joystick_run = true;
synchronized (this) {
if (joystick_run) {
pressed = 0;
notify();
}
}
}
public void joystickButtonChanged (Joystick j) {
but = j.getButtons();
joystick_run = true;
switch (but) {
case 0:
joystick_run = false;
break;
case 2:
translation = false;
orientation = true;
break;
// None
// Círculo
}
public void run () {
for (;;) {
pressed += 1;
switch (pov[0]) {
case -1:
// Izquierda
opengl_canvas.tEye -= 1 + pressed;
break;
case 1:
// Derecha
opengl_canvas.tEye += 1 + pressed;
break;
}
switch (pov[1]) {
case -1:
// Arriba
opengl_canvas.rEye += 0.5 + 0.5 * pressed;
break;
case 1:
// Abajo
opengl_canvas.rEye -= 0.5 + 0.5 * pressed;
break;
}
switch (but) {
case 1:
// Triángulo
opengl_canvas.pEye -= 1 + pressed;
break;
case 4:
// Cruz
opengl_canvas.pEye += 1 + pressed;
break;
case 16:
// L2
parallelRobot.setTheta (parallelRobot.getTheta(theta_chosen, Plataform.DEG) - (1 + pressed), theta_chosen,
Plataform.DEG);
if (opengl_canvas.showWorkspace)
pol.start ();
break;
case 32:
// R2
parallelRobot.setTheta (parallelRobot.getTheta(theta_chosen, Plataform.DEG) + (1 + pressed), theta_chosen,
Plataform.DEG);
218
if (opengl_canvas.showWorkspace)
pol.start ();
break;
}
if (translation) {
if (active_repos)
parallelRobot.setPosition (parallelRobot.getPx() + 0.3 * joyX, parallelRobot.getPy() + 0.3 * joyY, parallelRobot.getPz() + 0.4 *
joyZ);
if (active_reori)
parallelRobot.setGamma (parallelRobot.getGamma(Plataform.DEG) + 5 * joyR, Plataform.DEG);
}
else if (orientation) {
if (active_reori)
parallelRobot.setOrientation (parallelRobot.getAlfa(Plataform.DEG) + 5 * joyX, parallelRobot.getBeta(Plataform.DEG) + 5 *
joyY, parallelRobot.getGamma(Plataform.DEG) + 5 * joyR, Plataform.DEG);
if (active_repos)
parallelRobot.setPz (parallelRobot.getPz() + 0.4 * joyZ);
}
if (opengl_canvas.ready) {
updateBoxes ();
if (translation && joyR != joyRold && opengl_canvas.showWorkspace)
pol.start();
else if (orientation && (joyX != joyXold || joyY != joyYold || joyR != joyRold) && opengl_canvas.showWorkspace)
pol.start();
opengl_canvas.display ();
}
try {
Thread.sleep(10);
synchronized (this) {
while (!joystick_run)
wait();
}
} catch(InterruptedException e) {}
}
}
// Métodos generales
private double trunc (double x, int numDec) {
return ((double) Math.round(x * Math.pow(10, numDec))) / Math.pow(10, numDec);
}
void updateBoxes () {
// Para la posición
jtxtPx.setText (String.valueOf (trunc(parallelRobot.getPx(), 4)));
jtxtPy.setText (String.valueOf (trunc(parallelRobot.getPy(), 4)));
jtxtPz.setText (String.valueOf (trunc(parallelRobot.getPz(), 4)));
// Para la orientación
jtxtAlfa.setText (String.valueOf (trunc(parallelRobot.getAlfa(Plataform.DEG), 4)));
jtxtBeta.setText (String.valueOf (trunc(parallelRobot.getBeta(Plataform.DEG), 4)));
jtxtGamma.setText (String.valueOf (trunc(parallelRobot.getGamma(Plataform.DEG), 4)));
// Para los actuadores
for (int j = 0; j < 6; j++)
jtxtTheta[j].setText (String.valueOf (trunc(parallelRobot.getTheta(j + 1, Plataform.DEG), 4)));
opengl_canvas.display ();
}
public static JRadioButton getSelection (ButtonGroup group) {
for (Enumeration e = group.getElements(); e.hasMoreElements(); ) {
JRadioButton b = (JRadioButton)e.nextElement();
if (b.getModel() == group.getSelection())
return b;
}
return null;
}
}
219
D.7. Clase Matrix
private static double [][] Menores (double [][] A, int rx, int cx) {
class Matrix {
int rows = A.length;
int cols = A[0].length;
double [][] C = new double [rows][cols];
int h = 0, i;
static double [][] convMatrix (double [][] A, int rows, int cols) {
double [][] C = new double [rows][cols];
for (int j = 0; j < rows; j++)
for (int k = 0; k < cols; k++)
if (j < A.length && k < A[0].length)
C [j][k] = A [j][k];
else
C [j][k] = 0;
return C;
for (int j = 0; j < rows; j++) {
if (j != rx) {
i = 0;
for (int k = 0; k < cols; k++)
if (k != cx) {
C [h][i] = A [j][k];
i++;
}
h++;
}
}
return C;
}
static double [][] Suma (double [][] A, double [][] B) {
int rows = A.length;
int cols = A[0].length;
double [][] C = new double [rows][cols];
}
private static double Cofactores (double [][] A, int rows, int cols) {
for (int j = 0; j < rows; j++)
for (int k = 0; k < cols; k++)
C [j][k] = A [j][k] + convMatrix (B, rows, cols) [j][k];
return C;
double [][] M = new double [rows][cols];
double signo;
double det = 0;
}
if (rows == 1 && cols == 1)
return A [0][0];
else {
for (int k = 0; k < cols; k++) {
M = Menores (A, 0, k);
signo = Math.pow (-1.0, (double) k);
det += signo * A [0][k] * Cofactores (M, rows - 1, cols -
static double [][] Multiplica (double [][] A, double [][] B) {
int rows = A.length;
int rowcol = A[0].length;
int cols = B[0].length;
double [][] C = new double [rows][cols];
double [][] B_conv = convMatrix (B, rowcol, cols);
1);
}
return det;
}
for (int j = 0; j < rows; j++)
for (int k = 0; k < cols; k++)
for (int f = 0; f < rowcol; f++)
C [j][k] = C [j][k] + A [j][f] * B_conv [f][k];
return C;
}
static double det (double [][] A) {
return Cofactores (A, A.length, A[0].length);
}
}
static double [][] Multiplica (double [][] A, double b) {
static double [][] adj (double A [][]) {
int rows = A.length;
int cols = A[0].length;
double [][] C = new double [rows][cols];
int rows = A.length;
int cols = A[0].length;
double [][] C = new double [rows][cols];
double [][] M = new double [rows][cols];
double signo;
for (int j = 0; j < rows; j++)
for (int k = 0; k < cols; k++)
C [j][k] = b * A [j][k];
return C;
}
for (int j = 0; j < rows; j++)
for (int k = 0; k < cols; k++) {
M = Menores (A, j, k);
signo = Math.pow (-1.0, (double) j + k);
C [j][k] = signo * Cofactores (M, rows - 1, cols - 1);
}
return Trans (C);
static double [][] Trans (double [][] A) {
int rows = A.length;
int cols = A[0].length;
double [][] C = new double [rows][cols];
}
for (int j = 0; j < rows; j++)
for (int k = 0; k < cols; k++)
C [j][k] = A [k][j];
return C;
}
static double [][] inv (double A [][]) {
return Multiplica (adj(A), 1 / det(A));
}
}
D.8. Clase Vector
x = y = z = 0.0;
Public class Vector {
}
double x, y, z;
Vector () {
Vector (double x, double y, double z) {
this.x = x; this.y = y; this.z = z;
}
220
static Vector scalev (double a, Vector v) {
return new Vector (a * v.x, a * v.y, a * v.z);
}
static double norm (Vector v) {
return Math.sqrt (v.x * v.x + v.y * v.y + v.z * v.z);
}
static Vector sumv (Vector u, Vector v) {
return new Vector (u.x + v.x, u.y + v.y, u.z + v.z);
}
static Vector unitvec (Vector v) {
return scalev (1.0 / norm (v), v);
}
static Vector diffv (Vector u, Vector v) {
return new Vector (u.x - v.x, u.y - v.y, u.z - v.z);
}
static double dot (Vector u, Vector v) {
return (u.x * v.x + u.y * v.y + u.z * v.z);
}
static Vector lerpvec (Vector v1, Vector v2, double t) {
return sumv (v1, scalev (t, diffv (v2, v1)));
}
static Vector cross (Vector u, Vector v) {
return new Vector (u.y * v.z - u.z * v.y, u.z * v.x - u.x * v.z, u.x *
v.y - u.y * v.x);
}
}
D.9. Clase Polygon
public class Polygon {
Vector [] p = new Vector [3];
Vector [] n = new Vector [3];
Polygon (Vector p0, Vector p1, Vector p2, Vector n0, Vector n1, Vector n2) {
p [0] = p0;
p [1] = p1;
p [2] = p2;
n [0] = n0;
n [1] = n1;
n [2] = n2;
}
}
D.10. Clase Rotula
public class Rotula {
Vector nx;
Vector ny;
Vector nz;
}
D.11. Interface Implicit
public interface Implicit {
public double f_value (Vector p);
public Vector f_ngrad (Vector p);
}