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]));
}
Quaternion Quaternion::rotation(float angle, float x, float y, float z)
{
float len = (float)sqrt(x*x+y*y+z*z);
if(len!=0.0)
{
len = (float)(sin(angle/2.0f)/len);
return Quaternion((float) cos(angle/2.0f), x*len, y*len, z*len);
}
else
{
return Quaternion();
}
}
Quaternion Quaternion::rotation(float angle, const Vector& axis)
{
float len = (float)sqrt(axis[0]*axis[0]+axis[1]*axis[1]+axis[2]*axis[2]);
if(len!=0.0)
{
len = (float)(sin(angle/2.0f)/len);
return Quaternion((float) cos(angle/2.0f), axis[0]*len, axis[1]*len, axis[2]*len);
}
else
{
return Quaternion();
}
}
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
);
}
Quaternion Quaternion::operator*(const Quaternion& quat) const
{
return Quaternion(
p[0]*quat[0] - p[1]*quat[1] - p[2]*quat[2] - p[3]*quat[3],
p[0]*quat[1] + p[1]*quat[0] + p[2]*quat[3] - p[3]*quat[2],
p[0]*quat[2] - p[1]*quat[3] + p[2]*quat[0] + p[3]*quat[1],
p[0]*quat[3] + p[1]*quat[2] - p[2]*quat[1] + p[3]*quat[0]
);
}
Matrix Quaternion::buildMatrix() const
{
float w = p[0];
float x = p[1];
float y = p[2];
float z = p[3];
return Matrix(
1.0f-2.0f*y*y-2.0f*z*z, 2.0f*x*y-2.0f*w*z, 2.0f*x*z + 2.0f*w*y, 0.0f,
2.0f*x*y + 2.0f*w*z, 1.0f - 2.0f*x*x - 2.0f*z*z, 2.0f*y*z - 2.0f*w*x, 0.0f,
2.0f*x*z - 2.0f*w*y, 2.0f*y*z + 2.0f*w*x, 1.0f - 2.0f*x*x - 2.0f*y*y, 0.0f,
0.0f,0.0f,0.0f,1.0f
);
}
Quaternion Quaternion::power(double scalar)
{
float Dest[4];
double theta;
if(p[0]>=0.9999f)
{
theta = 0;
}
else if(p[0]<=-0.9999f)
{
theta = 2.0*3.1415926535897932384626433832795;
}
else
{
theta = acos(p[0]);
}
double u[3];
double scale = p[1]*p[1]+p[2]*p[2]+p[3]*p[3];
scale = sqrt(scale);
if(p[1]==0.0f && p[2]==0.0f && p[3]==0.0f)
{
u[0] = 0.0;
u[1] = 0.0;
u[2] = 0.0;
}
else
{
u[0] = p[1]/scale;
u[1] = p[2]/scale;
u[2] = p[3]/scale;
}
Dest[0] = (float)cos(scalar*theta);
Dest[1] = (float)(u[0] * sin(scalar*theta));
Dest[2] = (float)(u[1] * sin(scalar*theta));
Dest[3] = (float)(u[2] * sin(scalar*theta));
return Quaternion(Dest[0], Dest[1], Dest[2], Dest[3]);
}
Vector Vector::operator*(float scalar) const
{
return Vector(p[0]*scalar, p[1]*scalar, p[2]*scalar, p[3]);
}
Quaternion Quaternion::operator*(float scalar) const
{
return Quaternion(p[0]*scalar, p[1]*scalar, p[2]*scalar, p[3]*scalar);
}
Vector Quaternion::applyRotation(const Vector& vec) const
{
Quaternion result = (*this) * Quaternion(vec) * (conjugate());
return Vector(result[1], result[2], result[3], vec[3]);
}
Quaternion Quaternion::conjugate() const
{
return Quaternion(p[0], -p[1], -p[2], -p[3]);
}
Quaternion Quaternion::operator/(float scalar) const
{
return Quaternion(p[0]/scalar, p[1]/scalar, p[2]/scalar, p[3]/scalar);
}
Vector Vector::operator-() const
{
return Vector(-p[0], -p[1], -p[2], p[3]);
}
Vector Vector::badVector()
{
return Vector(0.0f, 0.0f, 0.0f, 0.0f);
}
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;
}
