import java.util.*;
public class Algorithm {
public static class Matrix{
private Double[][] x;
}
public static Scanner scan = new Scanner(System.in);
private static String str;
public static void read_data(Double[] radians, Double[] l) {
l[0] = 0.0;
int i;
for (i = 0; i < 9; i++) {
str = scan.next(); //passem la primera columna
str = scan.next(); //agafem el valor del desplaçament
str = str.substring(0, str.length()-1); //traiem la coma
l[i+1] = Double.parseDouble(str);
str = scan.next(); //passem la primera columna
str = scan.next(); //agafem el valor del desplaçament
if (i < 9) str = str.substring(0, str.length()-1); //traiem la coma
radians[i] = Math.toRadians(Double.parseDouble(str));
}
radians[i] = 0.0;
}
public static void print_Matrix(Double[][] M) {
for (int k = 0; k < 4; k++) {
System.out.print("\n");
for (int j = 0; j < 4; j++) {
System.out.print(String.valueOf(M[k][j]) + " , ");
}
}
}
public static void print_Jacobian(Double[][] M) {
System.out.print("Jacobian Matrix =");
for (int k = 0; k < 3; k++) {
System.out.print("\n");
for (int j = 0; j < 9; j++) {
System.out.print(String.valueOf(M[k][j]) + " , ");
}
}
}
public static void init_Matrix(Double[][] M, int i, Double[] radians, Double[] l) {
M[0][3] = l[i];
M[0][0] = Math.cos(radians[i]);
M[0][1] = -Math.sin(radians[i]);
M[1][0] = Math.sin(radians[i]);
M[1][1] = Math.cos(radians[i]);
for (int k = 0; k < 4; k++) {
for (int j = 0; j < 4; j++) {
if (k == j && (M[k][j] == null)) M[k][j] = 1.0;
else if(M[k][j] == null) M[k][j] = 0.0;
//System.out.print(String.valueOf(M[k][j]) + "\n");
}
}
}
public static void init_Ultima_Matrix(Double[][] M, int i, Double[] l) {
M[0][3] = l[i];
for (int k = 0; k < 4; k++) {
for (int j = 0; j < 4; j++) {
if (k == j) M[k][j] = 1.0;
else if(M[k][j] == null) M[k][j] = 0.0;
}
}
}
public static void init_Derivada(Double[][] M, int i, Double[] radians) {
M[0][0] = -Math.sin(radians[i]);
M[0][1] = -Math.cos(radians[i]);
M[1][0] = Math.cos(radians[i]);
M[1][1] = -Math.sin(radians[i]);
for (int k = 0; k < 4; k++) {
for (int j = 0; j < 4; j++) {
if(M[k][j] == null) M[k][j] = 0.0;
}
}
}
public static void init_Ultima_Derivada(Double[][] M, int i) {
for (int k = 0; k < 4; k++) {
for (int j = 0; j < 4; j++) {
M[k][j] = 0.0;
}
}
}
public static void fulfill_Ts(Matrix[] Ts, Double[] radians, Double[] l) {
int i;
for (i = 0; i < 9; i++) {
Ts[i].x = new Double[4][4];
init_Matrix(Ts[i].x, i, radians, l);
//print_Matrix(Ts[i].x);
}
init_Ultima_Matrix(Ts[i].x, i, l);
}
public static void fulfill_Ds(Matrix[] Ds, Double[] radians) {
int i;
for (i = 0; i < 9; i++) {
Ds[i].x = new Double[4][4];
init_Derivada(Ds[i].x, i, radians);
}
init_Ultima_Derivada(Ds[i].x, i);
}
private static Double[][] product(Double[][] A, Double[][] B){
Double suma = 0.0;
Double result[][] = new Double[4][4];
for(int i = 0; i < 4; i++){
for(int j = 0; j < 4; j++){
suma = 0.0;
for(int k = 0; k < 4; k++){
suma += A[i][k] * B[k][j];
}
result[i][j] = suma;
}
}
return result;
}
private static void calc_T(Matrix[] Ts, Double[][] T) {
T = Ts[0].x;
for (int j = 1; j < 10; j++) {
T = product(T, Ts[j].x);
}
}
private static void calc_Jacobian(Matrix[] Ts, Matrix[] Ds, int i, Double[][] jacobian) {
Double[][] tmp;
if (i == 0) tmp = Ds[0].x;
else tmp = Ts[0].x;
for (int j = 1; j < 10; j++) {
if (j == i) tmp = product(tmp, Ds[j].x);
else tmp = product(tmp, Ts[j].x);
}
jacobian[0][i] = tmp[0][3];
jacobian[1][i] = tmp[1][3];
jacobian[2][i] = tmp[0][0];
}
public static void main(String[] args) {
Matrix[] Ts = new Matrix[10];
Matrix[] Ds = new Matrix[10];
for (int i = 0; i < 10; i++) {
Ts[i] = new Matrix();
Ts[i].x = new Double[4][4];
Ds[i] = new Matrix();
Ds[i].x = new Double[4][4];
}
Double[] radians = new Double[10];
Double[] l = new Double[10];
read_data(radians, l);
fulfill_Ts(Ts, radians, l);
fulfill_Ds(Ds, radians);
Matrix T = new Matrix();
T.x = new Double[4][4];
System.out.print("\n Ts Matrix =");
for (int q=0; q<10; ++q) print_Matrix(Ts[q].x);
calc_T(Ts, T.x);
System.out.print("\n T Matrix =");
print_Matrix(T.x);
Matrix jacobian = new Matrix();
jacobian.x = new Double[3][9];
for (int j=0; j<9; j++)
calc_Jacobian(Ts, Ds, j, jacobian.x);
print_Jacobian(jacobian.x);
//La matriu Jacobiana hauria d'estar acabada
}
}
Hello everybody so here's my code. The problem is that in the function "calc_T(Matrix[] Ts, Double[][] T)", if I print the value of T before exit the function, it's OK; but if I do it once I've gone out of the function, it says:
T Matrix = null , null , null , null , null , null , null , null , null , null , null , null , null , null , null , null
Can anybody help me?
Thank you very much
This is the problem:
private static void calc_T(Matrix[] Ts, Double[][] T) {
T = Ts[0].x;
...
}
Arguments in Java are passed by value. So basically, you're ignoring the original value of T
, assigning a new value to it, and then doing nothing with it - that's not going to change anything for the caller. You want something like:
private static Double[][] calc_T(Matrix[] Ts) {
Double[] t = Ts[0].x;
for (int j = 1; j < 10; j++) {
t = product(t, Ts[j].x);
}
return t;
}
Then call it as:
T.x = calc_T(Ts);
You should also strongly consider using double[][]
instead of Double[][]
unless you need to be able to represent nulls, and start following Java naming conventions.
See more on this question at Stackoverflow