00001
00006 #include <bpm/bpm_orbit.h>
00007 #include <stdlib.h>
00008 #include <stdio.h>
00009 #include <math.h>
00010
00011 void v_copy(struct v3 *v1, struct v3 *v2) {
00012 v1->x = v2->x;
00013 v1->y = v2->y;
00014 v1->z = v2->z;
00015 }
00016
00017
00018 double v_mag(struct v3 *v1) {
00019 return sqrt(v_dot(v1,v1));
00020 }
00021
00022 void v_scale(struct v3 *v1, double dscale) {
00023 v1->x = v1->x*dscale;
00024 v1->y = v1->y*dscale;
00025 v1->z = v1->z*dscale;
00026 }
00027
00028 void v_norm(struct v3 *v1) {
00029 v_scale(v1,1/v_mag(v1));
00030 }
00031
00032 void v_matmult(struct m33 *m1, struct v3 *v1) {
00033 double x = 0;
00034 double y = 0;
00035 double z = 0;
00036 x = m1->e[0][0]*v1->x + m1->e[0][1]*v1->y + m1->e[0][2]*v1->z;
00037 y = m1->e[1][0]*v1->x + m1->e[1][1]*v1->y + m1->e[1][2]*v1->z;
00038 z = m1->e[2][0]*v1->x + m1->e[2][1]*v1->y + m1->e[2][2]*v1->z;
00039 v1->x = x;
00040 v1->y = y;
00041 v1->z = z;
00042 }
00043
00044 void v_add(struct v3 *v1, struct v3 *v2) {
00045 v1->x = v1->x + v2->x;
00046 v1->y = v1->y + v2->y;
00047 v1->z = v1->z + v2->z;
00048 }
00049
00050 void v_sub(struct v3 *v1, struct v3 *v2) {
00051 v1->x = v1->x - v2->x;
00052 v1->y = v1->y - v2->y;
00053 v1->z = v1->z - v2->z;
00054 }
00055
00056 double v_dot(struct v3 *v1, struct v3 *v2) {
00057 return v1->x*v2->x + v1->y*v2->y + v1->z*v2->z;
00058 }
00059
00060 void v_cross(struct v3 *v1, struct v3 *v2) {
00061 double x = 0;
00062 double y = 0;
00063 double z = 0;
00064
00065 x = v1->y*v2->z - v1->z*v2->y;
00066 y = -v1->x*v2->z + v1->z*v2->x;
00067 z = v1->x*v2->y - v1->y*v2->x;
00068
00069 v1->x = x;
00070 v1->y = y;
00071 v1->z = z;
00072 }
00073
00074 void v_print(struct v3 *v1) {
00075 printf("(%f,%f,%f)\n",v1->x,v1->y,v1->z);
00076 }
00077
00078 void m_rotmat(struct m33 *m1, double alpha, double beta, double gamma) {
00079 struct m33 *xrot;
00080 struct m33 *yrot;
00081 struct m33 *zrot;
00082 struct m33 *mtem;
00083
00084
00085
00086 xrot = calloc(1,sizeof(struct m33));
00087 yrot = calloc(1,sizeof(struct m33));
00088 zrot = calloc(1,sizeof(struct m33));
00089 mtem = calloc(1,sizeof(struct m33));
00090
00091 xrot->e[0][0] = 1.0;
00092 xrot->e[1][1] = cos(alpha);
00093 xrot->e[1][2] = sin(alpha);
00094 xrot->e[2][1] = -sin(alpha);
00095 xrot->e[2][2] = cos(alpha);
00096
00097 yrot->e[0][0] = cos(beta);
00098 yrot->e[0][2] = -sin(beta);
00099 yrot->e[1][1] = 1.0;
00100 yrot->e[2][0] = sin(beta);
00101 yrot->e[2][2] = cos(beta);
00102
00103 zrot->e[0][0] = cos(gamma);
00104 zrot->e[0][1] = sin(gamma);
00105 zrot->e[1][0] = -sin(gamma);
00106 zrot->e[1][1] = cos(gamma);
00107 zrot->e[2][2] = 1.0;
00108
00109
00110
00111
00112
00113 m_matmult(mtem,yrot,xrot);
00114 m_matmult(m1,zrot,mtem);
00115
00116
00117
00118
00119 free(xrot);
00120 free(yrot);
00121 free(zrot);
00122 free(mtem);
00123
00124 }
00125
00126 void m_matmult(struct m33 *m,struct m33 *m1, struct m33 *m2) {
00127 int i;
00128 int j;
00129 int k;
00130 for(i=0;i<3;i++) {
00131 for(j=0;j<3;j++) {
00132 m->e[i][j] = 0.0;
00133 for(k=0;k<3;k++) {
00134 m->e[i][j] += m1->e[i][k]*m2->e[k][j];
00135 }
00136 }
00137 }
00138 }
00139
00140 void m_matadd(struct m33 *m1, struct m33 *m2)
00141 {
00142 int i;
00143 int j;
00144 for(i=0;i<3;i++) {
00145 for(j=0;j<3;j++) {
00146 m1->e[i][j] = m1->e[i][j]+m2->e[i][j];
00147 }
00148 }
00149 }
00150
00151 void m_print(struct m33 *m1) {
00152 printf("(%f %f %f\n",m1->e[0][0],m1->e[0][1],m1->e[0][2]);
00153 printf(" %f %f %f\n",m1->e[1][0],m1->e[1][1],m1->e[1][2]);
00154 printf(" %f %f %f)\n",m1->e[2][0],m1->e[2][1],m1->e[2][2]);
00155 }