00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include <math.h>
00024 #include "vector3d.h"
00025 #include <stdio.h>
00026 #include "quaternion.h"
00027
00028 extern "C"{
00029 double cos(double);
00030 double sin(double);
00031 };
00032
00033
00034
00035 inline Vector3d::Vector3d(double _x, double _y, double _z){
00036 x=_x;
00037 y=_y;
00038 z=_z;
00039 }
00040
00041 inline string Vector3d::toString(void){
00042 char str[256];
00043
00044 sprintf(str, "(%g,%g,%g)",x,y,z);
00045
00046 return string(str);
00047 }
00048
00049
00050
00051
00052 inline Vector3d Vector3d::operator=(const Vector3d &b){
00053 x=b.x; y=b.y; z=b.z;
00054 return *this;
00055 }
00056
00057 inline Vector3d Vector3d::operator+=(const Vector3d &v){
00058 x+=v.x; y+=v.y; z+=v.z;
00059 return *this;
00060 }
00061
00062 inline Vector3d Vector3d::operator-=(const Vector3d &v){
00063 x-=v.x; y-=v.y; z-=v.z;
00064 return *this;
00065 }
00066
00067 inline Vector3d Vector3d::operator*=(double d){
00068 x*=d; y*=d; z*=d;
00069 return *this;
00070 }
00071
00072 inline Vector3d Vector3d::operator/=(double d){
00073 x/=d; y/=d; z/=d;
00074 return *this;
00075 }
00076
00077
00078
00079 inline double Vector3d::module(void){
00080 return sqrt(x*x+y*y+z*z);
00081 }
00082
00083 inline Vector3d Vector3d::normalized(void){
00084 double m=module();
00085 return Vector3d(x/m,y/m,z/m);
00086 }
00087
00088 inline void Vector3d::normalize(void){
00089 double m=module();
00090 x/=m;
00091 y/=m;
00092 z/=m;
00093 }
00094
00095
00096
00097
00098
00099 inline Vector3d Vector3d::rotate(const Vector3d &v, double ang){
00100
00101 double w,x,y,z;
00102 Vector3d V=v;
00103 V.normalize();
00104 w=cos(-ang/2);
00105
00106 double s=sin(-ang/2);
00107 x=V.x*s;
00108 y=V.y*s;
00109 z=V.z*s;
00110
00111 double w2=w*w;
00112 double x2=x*x;
00113 double y2=y*y;
00114 double z2=z*z;
00115
00116
00117 return Vector3d((*this).x*(w2+x2-y2-z2) + (*this).y*2*(x*y+w*z) + (*this).z*2*(x*z-w*y),
00118 (*this).x*2*(x*y-w*z) + (*this).y*(w2-x2+y2-z2) + (*this).z*2*(y*z+w*x),
00119 (*this).x*2*(x*z+w*y) + (*this).y*2*(y*z-w*x) + (*this).z*(w2-x2-y2+z2));
00120 }
00121
00122
00123 inline double Vector3d::X(){
00124 return x;
00125 }
00126
00127 inline double Vector3d::Y(){
00128 return y;
00129 }
00130
00131 inline double Vector3d::Z(){
00132 return z;
00133 }
00134
00135 inline double Vector3d::distToPlane(const Point3d &p, const Vector3d &n){
00136 Vector3d N=n;
00137 N.normalize();
00138
00139 return fabs(N*(*this-p));
00140 }
00141
00142
00143 inline double Vector3d::distToLine(const Point3d &p, const Vector3d &d){
00144 Vector3d v=(*this-p);
00145 Vector3d D=d;
00146 return (D^v).module()/D.module();
00147 }
00148
00149
00150
00151
00152 inline bool operator==(const Vector3d &a,const Vector3d &b){
00153 return (a.x==b.x)&&(a.y==b.y)&&(a.z==b.z);
00154 }
00155
00156
00157 inline double operator*(const Vector3d &a,const Vector3d &b){
00158 return a.x*b.x + a.y*b.y + a.z*b.z;
00159 }
00160
00161 inline Vector3d operator^(const Vector3d &a,const Vector3d &b){
00162 return Vector3d(a.y*b.z-a.z*b.y,
00163 -a.x*b.z+a.z*b.x,
00164 a.x*b.y-a.y*b.x);
00165 }
00166
00167 inline Vector3d operator+(const Vector3d &a,const Vector3d &b){
00168 return Vector3d(a.x+b.x, a.y+b.y, a.z+b.z);
00169 }
00170
00171 inline Vector3d operator-(const Vector3d &a,const Vector3d &b){
00172 return Vector3d(a.x-b.x, a.y-b.y, a.z-b.z);
00173 }
00174
00175 inline Vector3d operator-(const Vector3d &a){
00176 return Vector3d(-a.x, -a.y, -a.z);
00177 }
00178
00179 inline Vector3d operator*(const Vector3d &a,double d){
00180 return Vector3d(a.x*d, a.y*d, a.z*d);
00181 }
00182
00183 inline Vector3d operator/(const Vector3d &a, double d){
00184 return Vector3d(a.x/d, a.y/d, a.z/d);
00185 }
00186 inline Vector3d operator*(double d,const Vector3d &a){
00187 return Vector3d(a.x*d, a.y*d, a.z*d);
00188 }
00189
00190 inline Vector3d operator/(double d,const Vector3d &a){
00191 return Vector3d(a.x/d, a.y/d, a.z/d);
00192 }
00193
00194
00195 inline Vector3d Vector3d::operator=(const Quaternion &_q){
00196 Quaternion q(_q);
00197 *this=q.getAxis();
00198 return *this;
00199 }