| class View { | |
| public: | |
| View* next; | // Pointer for linked lists |
| private: | |
| double data[4]; | |
| public: | |
| View() { next=NULL; for ( int i = 0; i<4; i++ ) data[i] = 0; data[2]=1; } | // Initialize to {0,0,1,0} with a NULL pointer |
| View(const View& v) { for ( int i = 0; i<4; i++ ) data[i] = v.data[i]; normalize(); } | // Copies from another view, not the pointer |
| View(const double x, const double y, const double z, const double a) { | // Constructs from 4 values |
| next=NULL; data[0]=x; data[1]=y; data[2]=z; data[3]=a; normalize(); | |
| } | |
| // View from another form | |
| View(Quaternion& q) { | // Convert from a quaternion |
| double ang = batan2(q.z(), q.s()); | |
| double ca = bcos(ang), sa = bsin(ang); | |
| double q2 = q.s()*q.s() + q.z()*q.z(); | |
| double f2 = 2*sqrt(q2); | |
| if ( q2 < SMALLFLOAT ) ang = batan2(q.x(), q.y()); | |
| data[0] = f2*(q.y()*ca + q.x()*sa); | |
| data[1] = f2*(q.y()*sa - q.x()*ca); | |
| data[2] = 2*q2 - 1; | |
| data[3] = 2*ang; | |
| normalize(); | |
| next = NULL; | |
| } | |
| View(Matrix3 m) { | // Convert from a matrix |
| Quaternion q = m.quaternion(); | |
| *this = View(q); | |
| normalize(); | |
| } | |
| // Operators | |
| View operator=(const View& v) { for ( int i = 0; i<4; i++ ) data[i] = v.data[i]; return *this; } | // Copies the contents, not the pointer |
| View operator-() { | // Returns the negative of the view |
| View vn(-data[0], -data[1], -data[2], -data[3]); | |
| return vn; | |
| } | |
| bool operator==(const View& v) { | |
| int i, e = 0; | |
| normalize(); | |
| for ( i=0; i<4; i++ ) e += ( data[i] == v.data[i] ); | |
| return ( e == 4 ); | |
| } | |
| double& operator[](int i) { if ( i < 0 ) i = 0; if ( i > 3 ) i = 3; return data[i]; } | |
| double x() { return data[0]; } | |
| double y() { return data[1]; } | |
| double z() { return data[2]; } | |
| double a() { return data[3]; } | |
| void x(const double d) { data[0] = d; } | |
| void y(const double d) { data[1] = d; } | |
| void z(const double d) { data[2] = d; } | |
| void a(const double d) { data[3] = d; } | |
| void check() { | // Sets small numbers to zero and adjust the angle to [-PI,PI] |
| data[3] = angle_set_negPI_to_PI(data[3]); | |
| for ( int i = 0; i<4; i++ ) if ( fabs(data[i]) < TRIGPRECISION ) data[i] = 0; | |
| } | |
| double vector_size() { | // Returns the size of the vector |
| return sqrt(data[0]*data[0] + data[1]*data[1] + data[2]*data[2]); | |
| } | |
| double normalize() { | |
| double size = vector_size(); | |
| if ( size < SMALLFLOAT ) { data[0] = data[1] = 0; data[2] = 1; size = 1; } | |
| else { data[0] /= size; data[1] /= size; data[2] /= size; } | |
| check(); | |
| return size; | |
| } | |
| // View conversions | |
| void negate() { for ( int i = 0; i<4; i++ ) data[i] = -data[i]; } | // Negates the view |
| View backward() { | // Returns the backwards/inverse form of the view |
| double ca = bcos(data[3]); | |
| double sa = bsin(data[3]); | |
| View backview; | |
| backview.data[0] = -data[0]*ca - data[1]*sa; | |
| backview.data[1] = data[0]*sa - data[1]*ca; | |
| backview.data[2] = data[2]; | |
| backview.data[3] = angle_set_negPI_to_PI(-data[3]); | |
| backview.check(); | |
| return backview; | |
| } | |
| Quaternion quaternion() { | // Converts to a quaternion |
| normalize(); | |
| double ang = data[3]/2.0L; | |
| double ca = bcos(ang), sa = bsin(ang); | |
| double z1 = data[2] + 1, f1, f2; | |
| Quaternion q(ca, 0, 0, sa); | // z = 1 |
| if ( data[2] < 1.0L - SMALLFLOAT ) { | |
| if ( data[2] < SMALLFLOAT - 1.0L ) { | // z = -1 |
| q.s(0); | |
| q.x(sa); | |
| q.y(ca); | |
| q.z(0); | |
| } else { | // -1 < z < 1 |
| f1 = sqrt(z1/2.0); | |
| f2 = sqrt(1.0/(2*z1)); | |
| q.s(f1*ca); | |
| q.x(f2*(data[0]*sa - data[1]*ca)); | |
| q.y(f2*(data[0]*ca + data[1]*sa)); | |
| q.z(f1*sa); | |
| } | |
| } | |
| return q; | |
| } | |
| Matrix3 matrix() { | // Converts to a matrix |
| Quaternion q = quaternion(); | |
| return Matrix3(q); | |
| } | |
| // View comparisons | |
| double distance(const View& v) { | // Returns the distance between view vectors |
| double c, d = 0; | |
| for ( int i = 0; i<4; i++ ) { c = data[i] - v.data[i]; d += c*c; } | |
| if ( d > 0 ) d = sqrt(d); | |
| else d = 0; | |
| return(d); | |
| } | |
| double angle(const View& v) { | // Returns the angle between view vectors |
| double dot = 0; | |
| for ( int i = 0; i<3; i++ ) dot += data[i]*v.data[i]; | |
| return bacos(dot); | |
| } | |
| double residual(const View& v) { | // Returns a residual difference between views |
| double d = 1 - bcos(data[3] - v.data[3]); | |
| double r = d*d; | |
| for ( int i = 0; i<3; i++ ) { d = data[i] - v.data[i]; r += d*d; } | |
| r = sqrt(r/4); | |
| return r; | |
| } | |
| } ; |