00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 #include <stdio.h>
00042 #ifndef __UNIX_
00043 #include <malloc.h>
00044 #endif
00045 #include "mensagem.h"
00046 #include "memvirtu.h"
00047 #include "lowparam.h"
00048 #include "lowmacro.h"
00049 #include "lowsolid.h"
00050 #include "vectorop.h"
00051 #include "disdispl.h"
00052 #include "eulerops.h"
00053 #include "mancommd.h"
00054 #include "hiegroup.h"
00055
00056 #ifdef __Extended_Version
00057 void MSD_execNameRotateAxis(void)
00058 {
00059 char aname[30];
00060 float cx, cy, cz, ax, ay, az, th;
00061
00062 while (8 != sscanf(restbuf, "%s %f %f %f %f %f %f %f",
00063 aname, &cx, &cy, &cz,
00064 &ax, &ay, &az, &th))
00065 {
00066 printf("RotateAxis: axis cx cy cz ax ay az th\n");
00067 if (!lineins("? "))
00068 {
00069 return;
00070 }
00071 }
00072 MSD_highNameRotateAxis(aname, cx, cy, cz, ax, ay, az, th);
00073 }
00074
00075 #endif
00076
00077 int MSD_highNameRotateAxis(char *name, real cx, real cy, real cz,
00078 real ax, real ay, real az, real th)
00079 {
00080 int an;
00081
00082 if ((an = MSD_getAxisIdFromName(name)) == -1)
00083 {
00084 fprintf(stderr, MEN_NaoEncontrouAxisNome, NAM_RotateAxis, name);
00085 return(ERROR);
00086 }
00087 return(MSD_highRotateAxis(an, cx, cy, cz, ax, ay, az, th));
00088 }
00089
00090 #ifdef __Extended_Version
00091 void MSD_execRotateAxis(void)
00092 {
00093 float cx, cy, cz, ax, ay, az, th;
00094 int an;
00095
00096 while (8 != sscanf(restbuf, "%d %f %f %f %f %f %f %f",
00097 &an, &cx, &cy, &cz,
00098 &ax, &ay, &az, &th))
00099 {
00100 printf("RotateAxis: an cx cy cz ax ay az th\n");
00101 if (!lineins("? "))
00102 {
00103 return;
00104 }
00105 }
00106 MSD_highRotateAxis(an, cx, cy, cz, ax, ay, az, th);
00107 }
00108
00109 #endif
00110
00111 int MSD_highRotateAxis(int an, real cx, real cy, real cz,
00112 real ax, real ay, real az, real th)
00113 {
00114 APTYPE aptr;
00115 vector center, axis;
00116
00117 if ((aptr = MSD_getAxis(an)) == ANIL)
00118 {
00119 fprintf(stderr, MEN_NaoEncontrouAxisId, NAM_RotateAxis, an);
00120 return(ERROR);
00121 }
00122 makevec(center, cx, cy, cz, 1.0);
00123 makevec(axis, ax, ay, az, 1.0);
00124 return(MSD_lowRotateAxis(aptr, center, axis, th));
00125 }
00126
00127 int MSD_lowRotateAxis(APTYPE aptr, vector center, vector axis, real th)
00128 {
00129 real fd1, fd2, fd3, fd4;
00130 matrix rot;
00131 vector dis;
00132 Id an;
00133
00134 if (Gdoinversion)
00135 {
00136 an = AxsAxisNo(aptr);
00137 fd1 = center[0];
00138 fd2 = center[1];
00139 fd3 = center[2];
00140 fd4 = center[3];
00141 MSD_lowAddEulerOp(SNIL, ROTAXIS, -1, 0, 0, 0, 0, 0, 0, 0,
00142 fd1, fd2, fd3, fd4);
00143 fd1 = axis[0];
00144 fd2 = axis[1];
00145 fd3 = axis[2];
00146 fd4 = -th;
00147 MSD_lowAddEulerOp(SNIL, ROTAXIS, an, 0, 0, 0, 0, 0, 0, 0,
00148 fd1, fd2, fd3, fd4);
00149 }
00150
00151 rotmat(axis, th, rot);
00152 if (AxsMode(aptr) == AxisOfRotation)
00153 {
00154 vecmult(dis, center, rot);
00155 vecminus(dis, center, dis);
00156 vecmult(AxsCenter(aptr), AxsCenter(aptr), rot);
00157 vecplus(AxsCenter(aptr), dis, AxsCenter(aptr));
00158 }
00159 vecmult(AxsVector(aptr), AxsVector(aptr), rot);
00160 return(SUCCESS);
00161 }
00162
00163 int MSD_execStructRotateAxis(EulerOp *op)
00164 {
00165 EulerOp *op1;
00166 real fd1, fd2, fd3, fd4, fd5, fd6, fd7;
00167 Id an;
00168
00169 an = op->ip1;
00170 fd1 = op->fp1;
00171 fd2 = op->fp2;
00172 fd3 = op->fp3;
00173 fd7 = op->fp4;
00174
00175 #ifndef __VIRTUAL_MEM
00176 op1 = op->opnext;
00177 #else
00178 op1 = EULEROPS(op->pp - 1);
00179 #endif
00180
00181 fd4 = op1->fp1;
00182 fd5 = op1->fp2;
00183 fd6 = op1->fp3;
00184 return(MSD_highRotateAxis(an, fd1, fd2, fd3, fd4, fd5, fd6, fd7));
00185 }
00186
00187 void MSD_printStructRotateAxis(FILE *f, EulerOp *op)
00188 {
00189 Id sid, ip1;
00190 real fd1, fd2, fd3, fd4;
00191
00192 sid = op->solidno;
00193 ip1 = op->ip1;
00194 fd1 = op->fp1;
00195 fd2 = op->fp2;
00196 fd3 = op->fp3;
00197 fd4 = op->fp4;
00198 fprintf(f, "rotaxs %d %d %f %f %f %f\n", sid, ip1, fd1, fd2, fd3, fd4);
00199 }
00200
00201 int MSD_readStructRotateAxis(FILE *f, EulerOp *op)
00202 {
00203 int solidno, ip1;
00204 real fd1, fd2, fd3, fd4;
00205
00206 if (fscanf(f, "%d %d %f %f %f\n", &solidno, &ip1,
00207 &fd1, &fd2, &fd3, &fd4) != 6)
00208 {
00209 return(ERROR);
00210 }
00211 op->solidno = solidno;
00212 op->ip1 = ip1;
00213 op->ip2 = 0;
00214 op->ip3 = 0;
00215 op->ip4 = 0;
00216 op->ip5 = 0;
00217 op->ip6 = 0;
00218 op->ip7 = 0;
00219 op->ip8 = 0;
00220 op->fp1 = fd1;
00221 op->fp2 = fd2;
00222 op->fp3 = fd3;
00223 op->fp4 = fd4;
00224 return(SUCCESS);
00225 }
00226
00227 int MSD_modifyStructRotateAxis(EulerOp *op)
00228 {
00229 int axisno;
00230
00231 axisno = MSD_lowCheckTranslatorTable(op->ip1, AXIS);
00232 op->ip1 = axisno;
00233 return(SUCCESS);
00234 }