1 // **************************************************************************
2 // FILENAME: MatrixMy.cpp
3 // PURPOSE: Implementation of MatrixMy class
4 // CONTAINS:
5 // AUTHOR: Kenji Suzuki
6 // DATE: 18 June 1993
7 // COMMENT: Modified version of MatrixMy.C written by S L Davidson
8 // REVISION:
9 // **************************************************************************
10
11 #include <stdlib.h>
12 #include "MatrixMy.hpp"
13
14
15 // **************************************************************************
16 // Constructor
17 // **************************************************************************
MatrixMy()18 MatrixMy::MatrixMy()
19 {
20 row = 4;
21 column = 4;
22 m = new double *[4];
23
24 int i, j;
25 for (i = 0; i < 4; i++) {
26 m[i] = new double[4];
27 }
28
29 for (i = 0; i < 4; i++) {
30 for (j = 0; j < 4; j++) {
31 m[i][j] = 0.0;
32 }
33 }
34 }
35
36
37 // **************************************************************************
38 // Constructor
39 // **************************************************************************
40 //MatrixMy::MatrixMy(int r = 4, int c = 4)
MatrixMy(int r,int c)41 MatrixMy::MatrixMy(int r, int c)
42 {
43 row = r;
44 column = c;
45 m = new double *[row];
46
47 int i, j;
48 for (i = 0; i < row; i++) {
49 m[i] = new double[column];
50 }
51
52 for (i = 0; i < row; i++) {
53 for (j = 0; j < column; j++) {
54 m[i][j] = 0.0;
55 }
56 }
57 }
58
59
60 // **************************************************************************
61 // Copy Constructor
62 // **************************************************************************
MatrixMy(const MatrixMy & x)63 MatrixMy::MatrixMy(const MatrixMy &x)
64 {
65 int i, j;
66
67 row = x.row;
68 column = x.column;
69
70 m = new double *[row];
71 for (i = 0; i < row; i++) {
72 m[i] = new double[column];
73 }
74
75 for (i = 0; i < row; i++) {
76 for (j = 0; j < column; j++) {
77 m[i][j] = x.m[i][j];
78 }
79 }
80 }
81
82
83 // **************************************************************************
84 // Constructor fot Type Transfer(Vector -> MatrixMy)
85 // **************************************************************************
MatrixMy(Vector p)86 MatrixMy::MatrixMy(Vector p)
87 {
88 row = 3;
89 column = 1;
90 int i;
91
92 m = new double *[row];
93 for (i = 0; i < row; i++) {
94 m[i] = new double[column];
95 }
96
97 m[0][0] = p.x;
98 m[1][0] = p.y;
99 m[2][0] = p.z;
100 }
101
102
103 // **************************************************************************
104 // Destructor
105 // **************************************************************************
~MatrixMy()106 MatrixMy::~MatrixMy()
107 {
108 if (m != 0) {
109 int i;
110 for (i = 0; i < row; i++) {
111 delete m[i];
112 }
113 delete m;
114 }
115 }
116
117
118 // **************************************************************************
119 // substitution operator
120 // **************************************************************************
operator =(const MatrixMy & x)121 MatrixMy &MatrixMy::operator=(const MatrixMy &x)
122 {
123 // if ( (row == x.row) &&
124 // (column == x.column) ) {
125
126 int i, j;
127
128 row = x.row;
129 column = x.column;
130
131 m = new double *[row];
132 for (i = 0; i < row; i++) {
133 m[i] = new double[column];
134 }
135
136 for (i = 0; i < row; i++) {
137 for (j = 0; j < column; j++) {
138 m[i][j] = x.m[i][j];
139 }
140 }
141 return *this;
142 // }
143 // else {
144 // printf("ERROR: Matrix Dimension Not Match(= operator)\n");
145 // }
146 }
147
148
149 // **************************************************************************
150 // Type Transfer operator, Vector <- MatrixMy(3, 1)
151 // **************************************************************************
operator Vector()152 MatrixMy::operator Vector()
153 {
154 Vector p;
155
156 if ((row <= 4) && (column == 1)) {
157 p.x = m[0][0];
158 p.y = m[1][0];
159 p.z = m[2][0];
160 }
161 else {
162 printf("ERROR: Matrix Dimension Not Match(= MatrixMy->Vector)\n");
163 }
164 return (p);
165 }
166
167
168 // **************************************************************************
169 // "+" operator function
170 // **************************************************************************
operator +()171 MatrixMy MatrixMy::operator+()
172 {
173 MatrixMy temp(row, column);
174 int i, j;
175
176 for (i = 0; i < row; i++) {
177 for (j = 0; j < column; j++) {
178 temp.m[i][j] = m[i][j];
179 }
180 }
181
182 return (temp);
183
184 }
185
186
187 // **************************************************************************
188 // "-" operator function
189 // **************************************************************************
operator -()190 MatrixMy MatrixMy::operator-()
191 {
192 MatrixMy temp(row, column);
193 int i, j;
194
195 for (i = 0; i < row; i++) {
196 for (j = 0; j < column; j++) {
197 temp.m[i][j] = - m[i][j];
198 }
199 }
200
201 return (temp);
202
203 }
204
205
206 // **************************************************************************
207 // Friend "+" operator function
208 // **************************************************************************
operator +(const MatrixMy & left,const MatrixMy & right)209 MatrixMy operator+(const MatrixMy &left, const MatrixMy &right)
210 {
211 MatrixMy temp(left.row, left.column);
212 int i, j;
213
214 if ( (left.row == right.row) &&
215 (left.column == right.column) ) {
216
217 for (i = 0; i < temp.row; i++) {
218 for (j = 0; j < temp.column; j++) {
219 temp.m[i][j] = left.m[i][j] + right.m[i][j];
220 }
221 }
222
223 }
224 else {
225 printf("ERROR: Matrix Dimension Not Match(+ operator)\n");
226 }
227
228 return (temp);
229 }
230
231
232 // **************************************************************************
233 // Friend "-" operator function
234 // **************************************************************************
operator -(const MatrixMy & left,const MatrixMy & right)235 MatrixMy operator-(const MatrixMy &left, const MatrixMy &right)
236 {
237 MatrixMy temp(left.row, left.column);
238 int i, j;
239
240 if ( (left.row == right.row) &&
241 (left.column == right.column) ) {
242 for (i = 0; i < temp.row; i++) {
243 for (j = 0; j < temp.column; j++) {
244 temp.m[i][j] = left.m[i][j] - right.m[i][j];
245 }
246 }
247 }
248 else {
249 printf("ERROR: Matrix Dimension Not Match(- operator)\n");
250 }
251
252 return (temp);
253 }
254
255
256 // **************************************************************************
257 // Friend "*" operator function
258 // **************************************************************************
operator *(const MatrixMy & left,const MatrixMy & right)259 MatrixMy operator*(const MatrixMy &left, const MatrixMy &right)
260 {
261 MatrixMy temp(right.row, right.column);
262 int i, j, k;
263
264 if (left.column == right.row) {
265 for (i = 0; i < temp.row; i++) {
266 for (j = 0; j < temp.column; j++) {
267 for (k = 0; k < left.column; k++) {
268 temp.m[i][j] += left.m[i][k]*right.m[k][j];
269 }
270 }
271 }
272 }
273 else {
274 printf("ERROR: Matrix Dimension Not Match(* operator)\n");
275 }
276
277 return (temp);
278 }
279
280
281 // **************************************************************************
282 // Friend "*" operator function
283 // **************************************************************************
operator *(const MatrixMy & left,const double a)284 MatrixMy operator*(const MatrixMy &left, const double a)
285 {
286 MatrixMy temp(left.row, left.column);
287 int i, j;
288
289 for (i = 0; i < temp.row; i++) {
290 for (j = 0; j < temp.column; j++) {
291 temp.m[i][j] = a*left.m[i][j];
292 }
293 }
294
295 return (temp);
296 }
297
298
299 // **************************************************************************
300 // Friend "*" operator function
301 // **************************************************************************
operator *(const double a,const MatrixMy & right)302 MatrixMy operator*(const double a, const MatrixMy &right)
303 {
304 MatrixMy temp(right.row, right.column);
305 int i, j;
306
307 for (i = 0; i < temp.row; i++) {
308 for (j = 0; j < temp.column; j++) {
309 temp.m[i][j] = a*right.m[i][j];
310 }
311 }
312 return (temp);
313 }
314
315
316 // **************************************************************************
317 // Friend "*" operator function
318 // **************************************************************************
operator *(const MatrixMy & left,const Vector & right)319 Vector operator*(const MatrixMy &left, const Vector &right)
320 {
321 Vector temp;
322
323 // for Usual Matrix Multiplication
324 if ( (left.column == 3) && (left.row == 3) ) { // Mat(3x3) * Vec(3x1)
325 temp.x = left.m[0][0]*right.x
326 + left.m[0][1]*right.y
327 + left.m[0][2]*right.z;
328
329 temp.y = left.m[1][0]*right.x
330 + left.m[1][1]*right.y
331 + left.m[1][2]*right.z;
332
333 temp.z = left.m[2][0]*right.x
334 + left.m[2][1]*right.y
335 + left.m[2][2]*right.z;
336 }
337 // for Homogenious transformation
338 else if ( (left.column == 4) && (left.row == 4) ) {// Mat(4x4) * Vec(3x1)
339 temp.x = left.m[0][0]*right.x
340 + left.m[0][1]*right.y
341 + left.m[0][2]*right.z
342 + left.m[0][3];
343
344 temp.y = left.m[1][0]*right.x
345 + left.m[1][1]*right.y
346 + left.m[1][2]*right.z
347 + left.m[1][3];
348
349 temp.z = left.m[2][0]*right.x
350 + left.m[2][1]*right.y
351 + left.m[2][2]*right.z
352 + left.m[2][3];
353 }
354 else {
355 printf("ERROR: Matrix Dimension Not Match(M*V operator)\n");
356 }
357
358 return (temp);
359 }
360
361
362 #define TINY 1.0e-20;
363 // **************************************************************************
364 // Member Function: LU_Decomposition()
365 // PURPOSE: Compute LU Decomposition.
366 // Preparation for computation of Inverse Matrix.
367 // COMMENT: Modified ludcmp() function from "Numerical Recipes in C"
368 // **************************************************************************
LU_Decomposition(MatrixMy & indx,double & d)369 void MatrixMy::LU_Decomposition(MatrixMy &indx,
370 double &d)
371 {
372 int i, imax=0, j, k;
373 int n = row;
374 double big, dum, sum, temp;
375 MatrixMy vv(row, 1); // Vector
376
377 if ((column != row) ||
378 (indx.row != row) ||
379 (indx.column != 1)) {
380 printf("ERROR: Matrix Dimension Not Match(LUD)\n");
381 }
382 else {
383
384 d = 1.0;
385 for (i = 0; i < n; i++) {
386 big = 0.0;
387 for (j = 0; j < n; j++) {
388 if ( (temp = fabs(m[i][j])) > big ) {
389 big = temp;
390 }
391 }
392
393 if (big == 0.0) {
394 nrerror("Singular matrix in routine LUDCMP");
395 }
396 vv.m[i][0] = 1.0/big;
397 }
398
399 for (j = 0; j < n; j++) {
400 for (i = 0; i < j; i++) {
401 sum = m[i][j];
402 for (k = 0; k < i; k++) {
403 sum -= m[i][k]*m[k][j];
404 }
405 m[i][j] = sum;
406 }
407
408 big = 0.0;
409 for (i = j; i < n; i++) {
410 sum = m[i][j];
411 for (k = 0; k < j; k++) { // 1=>0
412 sum -= m[i][k]*m[k][j];
413 }
414
415 m[i][j] = sum;
416 if ( (dum = vv.m[i][0]*fabs(sum)) >= big ) {
417 big = dum;
418 imax = i;
419 }
420 }
421
422 if (j != (imax)) { //
423 for (k = 0; k < n; k++) { // 1=>0
424 dum = m[imax][k];
425 m[imax][k] = m[j][k];
426 m[j][k] = dum;
427 }
428 d = -d;
429 vv.m[imax][0] = vv.m[j][0];
430 }
431
432 indx.m[j][0] = imax;
433
434 if (m[j][j] == 0.0) {
435 m[j][j] = TINY;
436 }
437
438 if (j != (n - 1)) {
439 dum = 1.0/m[j][j];
440 for (i = (j + 1); i < n; i++) {
441 m[i][j] *= dum;
442 } // end of if()
443 } // end of if()
444 } // end of for()
445 } // end of if()
446
447 } //End of ludcmp()
448
449
450 // **************************************************************************
451 // Member Function: LU_Backsubstitution()
452 // PURPOSE: Compute LU Back substitution.
453 // Preparation for computation of Inverse Matrix.
454 // COMMENT: Modified lubksb() function from "Numerical Recipes in C"
455 // **************************************************************************
LU_Backsubstitution(MatrixMy & indx,MatrixMy & b)456 void MatrixMy::LU_Backsubstitution(MatrixMy &indx,
457 MatrixMy &b)
458 {
459 int i, ii = 0, ip, j;
460 int n = row;
461 double sum;
462
463 if (row != column) {
464 printf("ERROR: Matrix Dimension Not Match(LUB)\n");
465 }
466 else {
467
468 for (i = 0; i < n; i++) {
469 ip = (int)indx.m[i][0];
470 sum = b.m[ip][0];
471 b.m[ip][0] = b.m[i][0];
472
473 if (ii != 0) {
474 for (j = (ii - 1); j <= (i - 1); j++) {
475 sum -= m[i][j]*b.m[j][0];
476 }
477 }
478 else if (sum != 0.0) {
479 ii = i + 1;
480 }
481 b.m[i][0] = sum;
482 }
483
484 for (i = (n - 1); i >= 0; i--) {
485 sum = b.m[i][0];
486 for (j = (i + 1); j < n; j++) {
487 sum -= m[i][j]*b.m[j][0];
488 }
489 b.m[i][0] = sum/m[i][i];
490 }
491 }
492
493 } // End of LU_Backsubstitution()
494
495
496 // **************************************************************************
497 // Member function Inverse()
498 // PURPOSE: solve inverse matrix.
499 // **************************************************************************
Inverse()500 MatrixMy MatrixMy::Inverse()
501 {
502 MatrixMy temp(row, column);
503 MatrixMy ans(row, column);
504 MatrixMy col(row, 1);
505 MatrixMy indx(row, 1);
506 double d;
507 int i, j, n = row;
508
509 if (row == column) {
510
511 // copy matrix elements' value to temporal matrix
512 for (i = 0; i < n; i++) {
513 for (j = 0; j < n; j++) {
514 temp.m[i][j] = m[i][j];
515 }
516 }
517
518 temp.LU_Decomposition(indx, d);
519
520 for (j = 0; j < n; j++) {
521 for (i = 0; i < n; i++) {
522 col.m[i][0] = 0.0;
523 }
524 col.m[j][0] = 1.0;
525
526 temp.LU_Backsubstitution(indx, col);
527
528 for (i = 0; i < n; i++) {
529 ans.m[i][j] = col.m[i][0];
530 }
531 }
532 }
533 else {
534 printf("ERROR: Matrix Dimension Not Match(Inverse)\n");
535 }
536
537 return (ans);
538
539 } // End of Inverse()
540
541
542
543 // **************************************************************************
544 // Member function: Transpose()
545 // **************************************************************************
Transpose()546 MatrixMy MatrixMy::Transpose()
547 {
548 MatrixMy temp(column, row);
549 int i, j;
550
551 for (i = 0; i < temp.row; i++) {
552 for (j = 0; j < temp.column; j++) {
553 temp.m[i][j] = m[j][i];
554 }
555 }
556 return (temp);
557 }
558
559
560 // **************************************************************************
561 // Member function: Outerproduct()
562 // **************************************************************************
Outerproduct(MatrixMy & x)563 MatrixMy MatrixMy::Outerproduct(MatrixMy &x)
564 {
565 MatrixMy temp(3, 1);
566
567 if ( (row == x.row) && (column == x.column) &&
568 (x.row == 3) && (x.column == 1) ) {
569 temp.m[0][0] = m[1][0]*x.m[2][0] - m[2][0]*x.m[1][0];
570 temp.m[1][0] = m[2][0]*x.m[0][0] - m[0][0]*x.m[2][0];
571 temp.m[2][0] = m[0][0]*x.m[1][0] - m[1][0]*x.m[0][0];
572 }
573 else {
574 printf("ERROR: Matrix Dimension Not Match(outerproduct)\n");
575 }
576
577 return (temp);
578
579 } // End of Outerproduct()
580
581
582 // **************************************************************************
583 // Member function: Innerproduct()
584 // **************************************************************************
Innerproduct(MatrixMy & x)585 double MatrixMy::Innerproduct(MatrixMy &x)
586 {
587 double ans = 0.0;
588
589 if ( (row == x.row) && (column == x.column) &&
590 (x.row == 3) && (x.column == 1)) {
591 ans = m[0][0]*x.m[0][0] + m[1][0]*x.m[1][0] + m[2][0]*x.m[2][0];
592 ans = sqrt(ans);
593 }
594 else {
595 printf("ERROR: Matrix Dimension Not Match(innerproduct)\n");
596 }
597
598 return (ans);
599
600 } // End of Innerproduct()
601
602
603 // **************************************************************************
604 // Member function: Set_Value()
605 // COMMENT: Set Matrix elements
606 // **************************************************************************
Set_Value(int r,int c) const607 double &MatrixMy::Set_Value(int r, int c) const
608 {
609 if ( (r < row) &&
610 (c < column) ) {
611 return ( m[r][c] );
612 }
613 else {
614 fprintf(stderr, "ERROR: Matrix Dimension Not Match(set value)\n");
615 exit(1);
616
617 return m[0][0]; // keep some compilers from complaining
618 }
619 }
620
621
622 // **************************************************************************
623 // Member function: Get_Value()
624 // COMMENT: Set Matrix elements
625 // **************************************************************************
Get_Value(int r,int c)626 double MatrixMy::Get_Value(int r, int c)
627 {
628 if ( (r < row) &&
629 (c < column) ) {
630 return ( m[r][c] );
631 }
632 else {
633 fprintf(stderr,
634 "MatrixMy::Get_Value() error: Matrix Dimension Not Match\n");
635 exit(1);
636
637 return 0.0; // keep some of the compilers from complaining
638 }
639 }
640
641
642 // **************************************************************************
643 // Member function: Set_Vector3x1()
644 // COMMENT: Set 3 by 1 vector
645 // **************************************************************************
Set_Vector3x1(double x,double y,double z)646 void MatrixMy::Set_Vector3x1(double x, double y, double z)
647 {
648 if ( (row == 3) &&
649 (column == 1) ) {
650 Set_Value(0, 0) = x;
651 Set_Value(1, 0) = y;
652 Set_Value(2, 0) = z;
653 }
654 else {
655 printf("ERROR: Matrix Dimension Not Match(set vector)\n");
656 exit(1);
657 }
658 }
659
660 // **************************************************************************
661 // Member function: Set_Vector4x1()
662 // COMMENT: Set 4 by 1 Vector for Homogeneous Transformation
663 // **************************************************************************
Set_Vector4x1(double x,double y,double z)664 void MatrixMy::Set_Vector4x1(double x, double y, double z)
665 {
666 if ( (row == 4) &&
667 (column == 1) ) {
668 Set_Value(0, 0) = x;
669 Set_Value(1, 0) = y;
670 Set_Value(2, 0) = z;
671 Set_Value(3, 0) = 1.0;
672 }
673 else {
674 printf("ERROR: Matrix Dimension Not Match(set vector)\n");
675 exit(1);
676 }
677 }
678
679
680 // **************************************************************************
681 // Member function: Set_Hr_Matrix()
682 // COMMENT: Set Hr Rotation Matrix(3 by 3)
683 // X-Y-Z fixed angle, R_XYZ
684 // All rotation occur about axes of reference(WORLD) frame
685 // INPUT: rotation anlge about WORLD coordinates axes
686 // **************************************************************************
Set_Hr_Matrix(double roll,double elevation,double azimuth)687 void MatrixMy::Set_Hr_Matrix(double roll, double elevation, double azimuth)
688
689 {
690 if ( (row == 3) &&
691 (column == 3) ) {
692
693 double cz = cos(azimuth);
694 double sz = sin(azimuth);
695 double cy = cos(elevation);
696 double sy = sin(elevation);
697 double cx = cos(roll);
698 double sx = sin(roll);
699
700 Set_Value(0, 0) = (cz*cy);
701 Set_Value(1, 0) = (sz*cy);
702 Set_Value(2, 0) = (-sy);
703
704 Set_Value(0, 1) = (cz*sy*sx - sz*cx);
705 Set_Value(1, 1) = (sz*sy*sx + cz*cx);
706 Set_Value(2, 1) = (cy * sx);
707
708 Set_Value(0, 2) = (cz*sy*cx + sz*sx);
709 Set_Value(1, 2) = (sz*sy*cx - cz*sx);
710 Set_Value(2, 2) = (cy*cx);
711 }
712 else {
713 printf("ERROR: Matrix Dimension Not Match(set Hr matrix)\n");
714 }
715 } // End of Set_Hr_Matrix()
716
717
718 // **************************************************************************
719 // Member function
720 // COMMENT: Set Hr Rotation Matrix(4 by 4)
721 // X-Y-Z fixed angle, R_XYZ
722 // All rotation occur about axes of reference(WORLD) frame
723 // **************************************************************************
Set_H_Matrix(double x,double y,double z,double roll,double elevation,double azimuth)724 void MatrixMy::Set_H_Matrix(double x, double y, double z,
725 double roll, double elevation, double azimuth)
726 {
727 if ( (row == 4) &&
728 (column == 4) ) {
729
730 double cx = cos(roll);
731 double sx = sin(roll);
732 double cy = cos(elevation);
733 double sy = sin(elevation);
734 double cz = cos(azimuth);
735 double sz = sin(azimuth);
736
737 Set_Value(0, 0) = (cz*cy);
738 Set_Value(1, 0) = (sz*cy);
739 Set_Value(2, 0) = (-sy);
740 Set_Value(3, 0) = 0.0;
741
742 Set_Value(0, 1) = (cz*sy*sx - sz*cx);
743 Set_Value(1, 1) = (sz*sy*sx + cz*cx);
744 Set_Value(2, 1) = (cy * sx);
745 Set_Value(3, 1) = 0.0;
746
747 Set_Value(0, 2) = (cz*sy*cx + sz*sx);
748 Set_Value(1, 2) = (sz*sy*cx - cz*sx);
749 Set_Value(2, 2) = (cy*cx);
750 Set_Value(3, 2) = 0.0;
751
752 Set_Value(0, 3) = x;
753 Set_Value(1, 3) = y;
754 Set_Value(2, 3) = z;
755 Set_Value(3, 3) = 1.0;
756 }
757 else {
758 printf("ERROR: Matrix Dimension Not Match(set H matrix)\n");
759 }
760 } // End of Set_H_Matrix()
761
762
763 // **************************************************************************
764 // Member function: Set_DH_Matrix()
765 // COMMENT: Set Joint Transformation Matrix(4 by 4)
766 // X-Y-Z fixed angle, R_XYZ
767 // All rotation occur about axes of reference(WORLD) frame
768 // **************************************************************************
Set_DH_Matrix(double twist_angle,double link_length,double displacement,double joint_angle)769 void MatrixMy::Set_DH_Matrix(double twist_angle,
770 double link_length,
771 double displacement,
772 double joint_angle)
773 {
774 if ( (row == 4) &&
775 (column == 4) ) {
776
777 double ct, st;
778 double cj, sj;
779
780 ct = cos(twist_angle); st = sin(twist_angle);
781 cj = cos(joint_angle); sj = sin(joint_angle);
782
783 Set_Value(0, 0) = cj;
784 Set_Value(1, 0) = sj*ct;
785 Set_Value(2, 0) = sj*st;
786 Set_Value(3, 0) = 0.0;
787
788 Set_Value(0, 1) = -sj;
789 Set_Value(1, 1) = cj*ct;
790 Set_Value(2, 1) = cj*st;
791 Set_Value(3, 1) = 0.0;
792
793 Set_Value(0, 2) = 0.0;
794 Set_Value(1, 2) = -st;
795 Set_Value(2, 2) = ct;
796 Set_Value(3, 2) = 0.0;
797
798 Set_Value(0, 3) = link_length;
799 Set_Value(1, 3) =-st*displacement;
800 Set_Value(2, 3) = ct*displacement;
801 Set_Value(3, 3) = 1.0;
802 }
803 else {
804 printf("ERROR: Matrix Dimension Not Match(set DH matrix)\n");
805 }
806 } // End of Set_DH_Matrix()
807
808
809 // **************************************************************************
810 // Member function
811 // COMMENT: Set Jacobian Matrix(3 by 3)
812 // **************************************************************************
Set_J_Matrix(double theta0,double theta1,double theta2,double theta3)813 void MatrixMy::Set_J_Matrix(double theta0,
814 double theta1,
815 double theta2,
816 double theta3)
817 {
818 if ( (row == 3) &&
819 (column == 3) ) {
820
821 double c2, s2;
822 double c01, s01, c23, s23;
823
824 c2 = cos(theta2); s2 = sin(theta2);
825 c01 = cos(theta0 + theta1); s01 = sin(theta0 + theta1);
826 c23 = cos(theta2 + theta3); s23 = sin(theta2 + theta3);
827
828 Set_Value(0, 0) = -s01*(LINK1 + LINK2*c2 + LINK3*c23);
829 Set_Value(1, 0) = c01*(LINK1 + LINK2*c2 + LINK3*c23);
830 Set_Value(2, 0) = 0.0;
831
832 Set_Value(0, 1) = -c01*(LINK2*s2 + LINK3*s23);
833 Set_Value(1, 1) = -s01*(LINK2*s2 + LINK3*s23);
834 Set_Value(2, 1) = - (LINK2*c2 + LINK3*c23);
835
836 Set_Value(0, 2) = -LINK3*c01*s23;
837 Set_Value(1, 2) = -LINK3*s01*s23;
838 Set_Value(2, 2) = -LINK3*c23;
839 }
840 else {
841 printf("ERROR: Matrix Dimension Not Match(set J matrix)\n");
842 }
843 }
844
845
846 // **************************************************************************
847 // Member function: PrintMatrix()
848 // COMMENT: print out matrix elements
849 // **************************************************************************
PrintMatrix()850 void MatrixMy::PrintMatrix()
851 {
852 int i, j;
853
854 for (i = 0; i < row; i++) {
855 for (j = 0; j < column; j++) {
856 printf("%f ", m[i][j]);
857 }
858 printf("\n");
859 }
860 printf("\n");
861 }
862
863
864 // **************************************************************************
nrerror(char error_text[])865 void nrerror(char error_text[])
866 {
867 fprintf(stderr, "Numerical Recipes run time error...\n");
868 fprintf(stderr, "%s\n", error_text);
869 fprintf(stderr, "...now exiting to system...\n");
870 exit(1);
871
872 } // End of nrerror()
873
874
875 // EOF
876