Ray Quaternion::applyRotation(const Ray& ray) const { Quaternion origin = (*this) * Quaternion(ray.m_Origin) * (conjugate()); Quaternion dir = (*this) * Quaternion(ray.m_Dir) * (conjugate()); return Ray(Vector(origin[1], origin[2], origin[3], ray.m_Origin[3]), Vector(dir[1], dir[2], dir[3], ray.m_Dir[3])); }
Vector Vector::operator-(const Vector vec) const { return Vector( p[0]-vec[0], p[1]-vec[1], p[2]-vec[2], p[3]-vec[3]); }
Vector Vector::operator+(const Vector vec) const { return Vector( p[0]+vec[0], p[1]+vec[1], p[2]+vec[2], p[3]+vec[3]); }
Vector Vector::cross(const Vector& vec) const { return Vector( p[1]*vec[2] - p[2]*vec[1], p[2]*vec[0] - p[0]*vec[2], p[0]*vec[1] - p[1]*vec[0], 0.0f ); }
Vector Vector::badVector() { return Vector(0.0f, 0.0f, 0.0f, 0.0f); }
Vector Vector::operator-() const { return Vector(-p[0], -p[1], -p[2], p[3]); }
Vector Vector::operator*(float scalar) const { return Vector(p[0]*scalar, p[1]*scalar, p[2]*scalar, p[3]); }
Vector Quaternion::applyRotation(const Vector& vec) const { Quaternion result = (*this) * Quaternion(vec) * (conjugate()); return Vector(result[1], result[2], result[3], vec[3]); }
bool LinearAlgebra::getCylinderFit( int n, double* x, double* y, double* z, Vector* p1, Vector* p2, double* radius ) { // 1: Get the best fit Lxy on xy projection of points // 2: Get the best fit Lxz on xz projection of points // 3: Combine Lxy, Lxz to get the axis of cylinder // 4. Define the centroid to be a point on the axis // 5. Let radius of cyinder be average of radii of above two fits // 6. Some how get the extent of the axis double m1, m2, c1, c2, radius1, radius2; // step 1: if( !leastSquares( n, x, y, &m1, &c1, &radius1 ) ) return false; // step 2: if( !leastSquares( n, x, z, &m2, &c2, &radius2 ) ) return false; // step 3: // sin t = sqrt( m^2 / ( 1 + m^2 ) ) // cos t = sqrt( 1 / ( 1 + m^2 ) ) // The dcs of line 1 are norm( cos t1, sin t1, 0, 0 ) // The dcs of line 2 are norm( cos t2, 0, sin t2, 0 ) // The DCs are norm( cost1+cos t2, sint1, sin t2, 0 ) double sin_t1 = sqrt( m1*m1 / ( 1.0 + m1*m1) ); double cos_t1 = sqrt( 1 / ( 1.0 + m1*m1 ) ); if( m1 < 0 ) sin_t1 = -1* sin_t1; double sin_t2 = sqrt( m2*m2 / ( 1.0 + m2*m2) ); double cos_t2 = sqrt( 1 / ( 1.0 + m2*m2 ) ); if( m2 < 0 ) sin_t2 = -1*sin_t2; Vector DCs = Vector( (float)(cos_t1+cos_t2), (float)sin_t1, (float)sin_t2, 0 ); DCs.normalize(); // step 4: double x0, y0, z0; // point on axis ( say centroid ) if( !mean( x, n, &x0 ) ) return false; if( !mean( y, n, &y0 ) ) return false; if( !mean( z, n, &z0 ) ) return false; // step 5: *radius = ( radius1 + radius2 ) * 0.5; // step 6: // minLen = maxLen = 0 // for each point P // get norm ( p-p0 ) // compute cos t = dot prod ( axis dcs with prev result ) // len = len( ( p-p0 ) * cos t ) // update minLen, maxLen with len and sign of cos t // end // Endpoints are center + max Len, center - minLen double minLen = 0, maxLen = 0; int i; for( i=0; i<n; i++ ) { Vector diff( (float)(x[i] - x0), (float)(y[i] - y0), (float)(z[i] - z0), 0); Vector normdiff = diff; normdiff.normalize(); double cos_t = DCs.dot(normdiff); Vector proj( diff*(float)cos_t ); double len = proj.norm(); if( cos_t < 0 ) len *= -1; if( len < minLen ) minLen = len; if( len > maxLen ) maxLen = len; } // Endpoints are center + max Len, center - minLen p1->set((float)(x0+minLen*DCs[0]), (float)(y0+minLen*DCs[1]), (float)(z0+minLen*DCs[2]), 1); p2->set((float)(x0+maxLen*DCs[0]), (float)(y0+maxLen*DCs[1]), (float)(z0+maxLen*DCs[2]), 1); return true; }