dcbcc7ea3c
Defining a macro in a header that doesn't have any namespace prefix like GLH_ ugh. Very bad. Changed the macro into an inline function inside namespace glh.
1622 lines
35 KiB
C++
1622 lines
35 KiB
C++
/*
|
|
glh - is a platform-indepenedent C++ OpenGL helper library
|
|
|
|
|
|
Copyright (c) 2000 Cass Everitt
|
|
Copyright (c) 2000 NVIDIA Corporation
|
|
All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or
|
|
without modification, are permitted provided that the following
|
|
conditions are met:
|
|
|
|
* Redistributions of source code must retain the above
|
|
copyright notice, this list of conditions and the following
|
|
disclaimer.
|
|
|
|
* Redistributions in binary form must reproduce the above
|
|
copyright notice, this list of conditions and the following
|
|
disclaimer in the documentation and/or other materials
|
|
provided with the distribution.
|
|
|
|
* The names of contributors to this software may not be used
|
|
to endorse or promote products derived from this software
|
|
without specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
REGENTS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
|
|
Cass Everitt - cass@r3.nu
|
|
*/
|
|
|
|
/*
|
|
glh_linear.h
|
|
*/
|
|
|
|
// Author: Cass W. Everitt
|
|
|
|
#ifndef GLH_LINEAR_H
|
|
#define GLH_LINEAR_H
|
|
|
|
#include <memory.h>
|
|
#include <math.h>
|
|
#include <assert.h>
|
|
|
|
// only supports float for now...
|
|
#define GLH_REAL_IS_FLOAT
|
|
|
|
#ifdef GLH_REAL_IS_FLOAT
|
|
# define GLH_REAL float
|
|
# define GLH_REAL_NAMESPACE ns_float
|
|
#endif
|
|
|
|
#define GLH_QUATERNION_NORMALIZATION_THRESHOLD 64
|
|
|
|
#define GLH_RAD_TO_DEG GLH_REAL(57.2957795130823208767981548141052)
|
|
#define GLH_DEG_TO_RAD GLH_REAL(0.0174532925199432957692369076848861)
|
|
#define GLH_ZERO GLH_REAL(0.0)
|
|
#define GLH_ONE GLH_REAL(1.0)
|
|
#define GLH_TWO GLH_REAL(2.0)
|
|
#define GLH_EPSILON GLH_REAL(10e-6)
|
|
#define GLH_PI GLH_REAL(3.1415926535897932384626433832795)
|
|
|
|
|
|
namespace glh
|
|
{
|
|
inline bool equivalent(GLH_REAL a, GLH_REAL b) { return b - GLH_EPSILON < a && a < b + GLH_EPSILON; }
|
|
|
|
inline GLH_REAL to_degrees(GLH_REAL radians) { return radians*GLH_RAD_TO_DEG; }
|
|
inline GLH_REAL to_radians(GLH_REAL degrees) { return degrees*GLH_DEG_TO_RAD; }
|
|
|
|
// forward declarations for friend template functions.
|
|
template <int N, class T> class vec;
|
|
|
|
// forward declarations for friend template functions.
|
|
template <int N, class T>
|
|
bool operator == ( const vec<N,T> & v1, const vec<N,T> & v2 );
|
|
|
|
// forward declarations for friend template functions.
|
|
template <int N, class T>
|
|
bool operator != ( const vec<N,T> & v1, const vec<N,T> & v2 );
|
|
|
|
template <int N, class T>
|
|
class vec
|
|
{
|
|
public:
|
|
int size() const { return N; }
|
|
|
|
vec(const T & t = T())
|
|
{ for(int i = 0; i < N; i++) v[i] = t; }
|
|
vec(const T * tp)
|
|
{ for(int i = 0; i < N; i++) v[i] = tp[i]; }
|
|
|
|
const T * get_value() const
|
|
{ return v; }
|
|
|
|
|
|
T dot( const vec<N,T> & rhs ) const
|
|
{
|
|
T r = 0;
|
|
for(int i = 0; i < N; i++) r += v[i]*rhs.v[i];
|
|
return r;
|
|
}
|
|
|
|
T length() const
|
|
{
|
|
T r = 0;
|
|
for(int i = 0; i < N; i++) r += v[i]*v[i];
|
|
return T(sqrt(r));
|
|
}
|
|
|
|
T square_norm() const
|
|
{
|
|
T r = 0;
|
|
for(int i = 0; i < N; i++) r += v[i]*v[i];
|
|
return r;
|
|
}
|
|
|
|
void negate()
|
|
{ for(int i = 0; i < N; i++) v[i] = -v[i]; }
|
|
|
|
|
|
T normalize()
|
|
{
|
|
T sum(0);
|
|
for(int i = 0; i < N; i++)
|
|
sum += v[i]*v[i];
|
|
sum = T(sqrt(sum));
|
|
if (sum > GLH_EPSILON)
|
|
for(int i = 0; i < N; i++)
|
|
v[i] /= sum;
|
|
return sum;
|
|
}
|
|
|
|
|
|
vec<N,T> & set_value( const T * rhs )
|
|
{ for(int i = 0; i < N; i++) v[i] = rhs[i]; return *this; }
|
|
|
|
T & operator [] ( int i )
|
|
{ return v[i]; }
|
|
|
|
const T & operator [] ( int i ) const
|
|
{ return v[i]; }
|
|
|
|
vec<N,T> & operator *= ( T d )
|
|
{ for(int i = 0; i < N; i++) v[i] *= d; return *this;}
|
|
|
|
vec<N,T> & operator *= ( const vec<N,T> & u )
|
|
{ for(int i = 0; i < N; i++) v[i] *= u[i]; return *this;}
|
|
|
|
vec<N,T> & operator /= ( T d )
|
|
{ if(d == 0) return *this; for(int i = 0; i < N; i++) v[i] /= d; return *this;}
|
|
|
|
vec<N,T> & operator += ( const vec<N,T> & u )
|
|
{ for(int i = 0; i < N; i++) v[i] += u.v[i]; return *this;}
|
|
|
|
vec<N,T> & operator -= ( const vec<N,T> & u )
|
|
{ for(int i = 0; i < N; i++) v[i] -= u.v[i]; return *this;}
|
|
|
|
|
|
vec<N,T> operator - () const
|
|
{ vec<N,T> rv = v; rv.negate(); return rv; }
|
|
|
|
vec<N,T> operator + ( const vec<N,T> &v) const
|
|
{ vec<N,T> rt(*this); return rt += v; }
|
|
|
|
vec<N,T> operator - ( const vec<N,T> &v) const
|
|
{ vec<N,T> rt(*this); return rt -= v; }
|
|
|
|
vec<N,T> operator * ( T d) const
|
|
{ vec<N,T> rt(*this); return rt *= d; }
|
|
|
|
friend bool operator == <> ( const vec<N,T> &v1, const vec<N,T> &v2 );
|
|
friend bool operator != <> ( const vec<N,T> &v1, const vec<N,T> &v2 );
|
|
|
|
|
|
//protected:
|
|
T v[N];
|
|
};
|
|
|
|
|
|
|
|
// vector friend operators
|
|
|
|
template <int N, class T> inline
|
|
vec<N,T> operator * ( const vec<N,T> & b, T d )
|
|
{
|
|
vec<N,T> rt(b);
|
|
return rt *= d;
|
|
}
|
|
|
|
template <int N, class T> inline
|
|
vec<N,T> operator * ( T d, const vec<N,T> & b )
|
|
{ return b*d; }
|
|
|
|
template <int N, class T> inline
|
|
vec<N,T> operator * ( const vec<N,T> & b, const vec<N,T> & d )
|
|
{
|
|
vec<N,T> rt(b);
|
|
return rt *= d;
|
|
}
|
|
|
|
template <int N, class T> inline
|
|
vec<N,T> operator / ( const vec<N,T> & b, T d )
|
|
{ vec<N,T> rt(b); return rt /= d; }
|
|
|
|
template <int N, class T> inline
|
|
vec<N,T> operator + ( const vec<N,T> & v1, const vec<N,T> & v2 )
|
|
{ vec<N,T> rt(v1); return rt += v2; }
|
|
|
|
template <int N, class T> inline
|
|
vec<N,T> operator - ( const vec<N,T> & v1, const vec<N,T> & v2 )
|
|
{ vec<N,T> rt(v1); return rt -= v2; }
|
|
|
|
|
|
template <int N, class T> inline
|
|
bool operator == ( const vec<N,T> & v1, const vec<N,T> & v2 )
|
|
{
|
|
for(int i = 0; i < N; i++)
|
|
if(v1.v[i] != v2.v[i])
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
template <int N, class T> inline
|
|
bool operator != ( const vec<N,T> & v1, const vec<N,T> & v2 )
|
|
{ return !(v1 == v2); }
|
|
|
|
|
|
typedef vec<3,unsigned char> vec3ub;
|
|
typedef vec<4,unsigned char> vec4ub;
|
|
|
|
|
|
|
|
|
|
|
|
namespace GLH_REAL_NAMESPACE
|
|
{
|
|
typedef GLH_REAL real;
|
|
|
|
class line;
|
|
class plane;
|
|
class matrix4;
|
|
class quaternion;
|
|
typedef quaternion rotation;
|
|
|
|
class vec2 : public vec<2,real>
|
|
{
|
|
public:
|
|
vec2(const real & t = real()) : vec<2,real>(t)
|
|
{}
|
|
vec2(const vec<2,real> & t) : vec<2,real>(t)
|
|
{}
|
|
vec2(const real * tp) : vec<2,real>(tp)
|
|
{}
|
|
|
|
vec2(real x, real y )
|
|
{ v[0] = x; v[1] = y; }
|
|
|
|
void get_value(real & x, real & y) const
|
|
{ x = v[0]; y = v[1]; }
|
|
|
|
vec2 & set_value( const real & x, const real & y)
|
|
{ v[0] = x; v[1] = y; return *this; }
|
|
|
|
};
|
|
|
|
|
|
class vec3 : public vec<3,real>
|
|
{
|
|
public:
|
|
vec3(const real & t = real()) : vec<3,real>(t)
|
|
{}
|
|
vec3(const vec<3,real> & t) : vec<3,real>(t)
|
|
{}
|
|
vec3(const real * tp) : vec<3,real>(tp)
|
|
{}
|
|
|
|
vec3(real x, real y, real z)
|
|
{ v[0] = x; v[1] = y; v[2] = z; }
|
|
|
|
void get_value(real & x, real & y, real & z) const
|
|
{ x = v[0]; y = v[1]; z = v[2]; }
|
|
|
|
vec3 cross( const vec3 &rhs ) const
|
|
{
|
|
vec3 rt;
|
|
rt.v[0] = v[1]*rhs.v[2]-v[2]*rhs.v[1];
|
|
rt.v[1] = v[2]*rhs.v[0]-v[0]*rhs.v[2];
|
|
rt.v[2] = v[0]*rhs.v[1]-v[1]*rhs.v[0];
|
|
return rt;
|
|
}
|
|
|
|
vec3 & set_value( const real & x, const real & y, const real & z)
|
|
{ v[0] = x; v[1] = y; v[2] = z; return *this; }
|
|
|
|
};
|
|
|
|
|
|
class vec4 : public vec<4,real>
|
|
{
|
|
public:
|
|
vec4(const real & t = real()) : vec<4,real>(t)
|
|
{}
|
|
vec4(const vec<4,real> & t) : vec<4,real>(t)
|
|
{}
|
|
|
|
vec4(const vec<3,real> & t, real fourth)
|
|
|
|
{ v[0] = t.v[0]; v[1] = t.v[1]; v[2] = t.v[2]; v[3] = fourth; }
|
|
vec4(const real * tp) : vec<4,real>(tp)
|
|
{}
|
|
vec4(real x, real y, real z, real w)
|
|
{ v[0] = x; v[1] = y; v[2] = z; v[3] = w; }
|
|
|
|
void get_value(real & x, real & y, real & z, real & w) const
|
|
{ x = v[0]; y = v[1]; z = v[2]; w = v[3]; }
|
|
|
|
vec4 & set_value( const real & x, const real & y, const real & z, const real & w)
|
|
{ v[0] = x; v[1] = y; v[2] = z; v[3] = w; return *this; }
|
|
};
|
|
|
|
inline
|
|
vec3 homogenize(const vec4 & v)
|
|
{
|
|
vec3 rt;
|
|
assert(v.v[3] != GLH_ZERO);
|
|
rt.v[0] = v.v[0]/v.v[3];
|
|
rt.v[1] = v.v[1]/v.v[3];
|
|
rt.v[2] = v.v[2]/v.v[3];
|
|
return rt;
|
|
}
|
|
|
|
|
|
|
|
class line
|
|
{
|
|
public:
|
|
|
|
line()
|
|
{ set_value(vec3(0,0,0),vec3(0,0,1)); }
|
|
|
|
line( const vec3 & p0, const vec3 &p1)
|
|
{ set_value(p0,p1); }
|
|
|
|
void set_value( const vec3 &p0, const vec3 &p1)
|
|
{
|
|
position = p0;
|
|
direction = p1-p0;
|
|
direction.normalize();
|
|
}
|
|
|
|
bool get_closest_points(const line &line2,
|
|
vec3 &pointOnThis,
|
|
vec3 &pointOnThat)
|
|
{
|
|
|
|
// quick check to see if parallel -- if so, quit.
|
|
if(fabs(direction.dot(line2.direction)) == 1.0)
|
|
return 0;
|
|
line l2 = line2;
|
|
|
|
// Algorithm: Brian Jean
|
|
//
|
|
register real u;
|
|
register real v;
|
|
vec3 Vr = direction;
|
|
vec3 Vs = l2.direction;
|
|
register real Vr_Dot_Vs = Vr.dot(Vs);
|
|
register real detA = real(1.0 - (Vr_Dot_Vs * Vr_Dot_Vs));
|
|
vec3 C = l2.position - position;
|
|
register real C_Dot_Vr = C.dot(Vr);
|
|
register real C_Dot_Vs = C.dot(Vs);
|
|
|
|
u = (C_Dot_Vr - Vr_Dot_Vs * C_Dot_Vs)/detA;
|
|
v = (C_Dot_Vr * Vr_Dot_Vs - C_Dot_Vs)/detA;
|
|
|
|
pointOnThis = position;
|
|
pointOnThis += direction * u;
|
|
pointOnThat = l2.position;
|
|
pointOnThat += l2.direction * v;
|
|
|
|
return 1;
|
|
}
|
|
|
|
vec3 get_closest_point(const vec3 &point)
|
|
{
|
|
vec3 np = point - position;
|
|
vec3 rp = direction*direction.dot(np)+position;
|
|
return rp;
|
|
}
|
|
|
|
const vec3 & get_position() const {return position;}
|
|
|
|
const vec3 & get_direction() const {return direction;}
|
|
|
|
//protected:
|
|
vec3 position;
|
|
vec3 direction;
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// matrix
|
|
|
|
|
|
class matrix4
|
|
{
|
|
|
|
public:
|
|
|
|
matrix4() { make_identity(); }
|
|
|
|
matrix4( real r )
|
|
{ set_value(r); }
|
|
|
|
matrix4( real * m )
|
|
{ set_value(m); }
|
|
|
|
matrix4( real a00, real a01, real a02, real a03,
|
|
real a10, real a11, real a12, real a13,
|
|
real a20, real a21, real a22, real a23,
|
|
real a30, real a31, real a32, real a33 )
|
|
{
|
|
element(0,0) = a00;
|
|
element(0,1) = a01;
|
|
element(0,2) = a02;
|
|
element(0,3) = a03;
|
|
|
|
element(1,0) = a10;
|
|
element(1,1) = a11;
|
|
element(1,2) = a12;
|
|
element(1,3) = a13;
|
|
|
|
element(2,0) = a20;
|
|
element(2,1) = a21;
|
|
element(2,2) = a22;
|
|
element(2,3) = a23;
|
|
|
|
element(3,0) = a30;
|
|
element(3,1) = a31;
|
|
element(3,2) = a32;
|
|
element(3,3) = a33;
|
|
}
|
|
|
|
|
|
void get_value( real * mp ) const
|
|
{
|
|
int c = 0;
|
|
for(int j=0; j < 4; j++)
|
|
for(int i=0; i < 4; i++)
|
|
mp[c++] = element(i,j);
|
|
}
|
|
|
|
|
|
const real * get_value() const
|
|
{ return m; }
|
|
|
|
void set_value( real * mp)
|
|
{
|
|
int c = 0;
|
|
for(int j=0; j < 4; j++)
|
|
for(int i=0; i < 4; i++)
|
|
element(i,j) = mp[c++];
|
|
}
|
|
|
|
void set_value( real r )
|
|
{
|
|
for(int i=0; i < 4; i++)
|
|
for(int j=0; j < 4; j++)
|
|
element(i,j) = r;
|
|
}
|
|
|
|
void make_identity()
|
|
{
|
|
element(0,0) = 1.0;
|
|
element(0,1) = 0.0;
|
|
element(0,2) = 0.0;
|
|
element(0,3) = 0.0;
|
|
|
|
element(1,0) = 0.0;
|
|
element(1,1) = 1.0;
|
|
element(1,2) = 0.0;
|
|
element(1,3) = 0.0;
|
|
|
|
element(2,0) = 0.0;
|
|
element(2,1) = 0.0;
|
|
element(2,2) = 1.0;
|
|
element(2,3) = 0.0;
|
|
|
|
element(3,0) = 0.0;
|
|
element(3,1) = 0.0;
|
|
element(3,2) = 0.0;
|
|
element(3,3) = 1.0;
|
|
}
|
|
|
|
|
|
static matrix4 identity()
|
|
{
|
|
static matrix4 mident (
|
|
1.0, 0.0, 0.0, 0.0,
|
|
0.0, 1.0, 0.0, 0.0,
|
|
0.0, 0.0, 1.0, 0.0,
|
|
0.0, 0.0, 0.0, 1.0 );
|
|
return mident;
|
|
}
|
|
|
|
|
|
void set_scale( real s )
|
|
{
|
|
element(0,0) = s;
|
|
element(1,1) = s;
|
|
element(2,2) = s;
|
|
}
|
|
|
|
void set_scale( const vec3 & s )
|
|
{
|
|
element(0,0) = s.v[0];
|
|
element(1,1) = s.v[1];
|
|
element(2,2) = s.v[2];
|
|
}
|
|
|
|
|
|
void set_translate( const vec3 & t )
|
|
{
|
|
element(0,3) = t.v[0];
|
|
element(1,3) = t.v[1];
|
|
element(2,3) = t.v[2];
|
|
}
|
|
|
|
void set_row(int r, const vec4 & t)
|
|
{
|
|
element(r,0) = t.v[0];
|
|
element(r,1) = t.v[1];
|
|
element(r,2) = t.v[2];
|
|
element(r,3) = t.v[3];
|
|
}
|
|
|
|
void set_column(int c, const vec4 & t)
|
|
{
|
|
element(0,c) = t.v[0];
|
|
element(1,c) = t.v[1];
|
|
element(2,c) = t.v[2];
|
|
element(3,c) = t.v[3];
|
|
}
|
|
|
|
|
|
void get_row(int r, vec4 & t) const
|
|
{
|
|
t.v[0] = element(r,0);
|
|
t.v[1] = element(r,1);
|
|
t.v[2] = element(r,2);
|
|
t.v[3] = element(r,3);
|
|
}
|
|
|
|
vec4 get_row(int r) const
|
|
{
|
|
vec4 v; get_row(r, v);
|
|
return v;
|
|
}
|
|
|
|
void get_column(int c, vec4 & t) const
|
|
{
|
|
t.v[0] = element(0,c);
|
|
t.v[1] = element(1,c);
|
|
t.v[2] = element(2,c);
|
|
t.v[3] = element(3,c);
|
|
}
|
|
|
|
vec4 get_column(int c) const
|
|
{
|
|
vec4 v; get_column(c, v);
|
|
return v;
|
|
}
|
|
|
|
matrix4 inverse() const
|
|
{
|
|
matrix4 minv;
|
|
|
|
real r1[8], r2[8], r3[8], r4[8];
|
|
real *s[4], *tmprow;
|
|
|
|
s[0] = &r1[0];
|
|
s[1] = &r2[0];
|
|
s[2] = &r3[0];
|
|
s[3] = &r4[0];
|
|
|
|
register int i,j,p,jj;
|
|
for(i=0;i<4;i++)
|
|
{
|
|
for(j=0;j<4;j++)
|
|
{
|
|
s[i][j] = element(i,j);
|
|
if(i==j) s[i][j+4] = 1.0;
|
|
else s[i][j+4] = 0.0;
|
|
}
|
|
}
|
|
real scp[4];
|
|
for(i=0;i<4;i++)
|
|
{
|
|
scp[i] = real(fabs(s[i][0]));
|
|
for(j=1;j<4;j++)
|
|
if(real(fabs(s[i][j])) > scp[i]) scp[i] = real(fabs(s[i][j]));
|
|
if(scp[i] == 0.0) return minv; // singular matrix!
|
|
}
|
|
|
|
int pivot_to;
|
|
real scp_max;
|
|
for(i=0;i<4;i++)
|
|
{
|
|
// select pivot row
|
|
pivot_to = i;
|
|
scp_max = real(fabs(s[i][i]/scp[i]));
|
|
// find out which row should be on top
|
|
for(p=i+1;p<4;p++)
|
|
if(real(fabs(s[p][i]/scp[p])) > scp_max)
|
|
{ scp_max = real(fabs(s[p][i]/scp[p])); pivot_to = p; }
|
|
// Pivot if necessary
|
|
if(pivot_to != i)
|
|
{
|
|
tmprow = s[i];
|
|
s[i] = s[pivot_to];
|
|
s[pivot_to] = tmprow;
|
|
real tmpscp;
|
|
tmpscp = scp[i];
|
|
scp[i] = scp[pivot_to];
|
|
scp[pivot_to] = tmpscp;
|
|
}
|
|
|
|
real mji;
|
|
// perform gaussian elimination
|
|
for(j=i+1;j<4;j++)
|
|
{
|
|
mji = s[j][i]/s[i][i];
|
|
s[j][i] = 0.0;
|
|
for(jj=i+1;jj<8;jj++)
|
|
s[j][jj] -= mji*s[i][jj];
|
|
}
|
|
}
|
|
if(s[3][3] == 0.0) return minv; // singular matrix!
|
|
|
|
//
|
|
// Now we have an upper triangular matrix.
|
|
//
|
|
// x x x x | y y y y
|
|
// 0 x x x | y y y y
|
|
// 0 0 x x | y y y y
|
|
// 0 0 0 x | y y y y
|
|
//
|
|
// we'll back substitute to get the inverse
|
|
//
|
|
// 1 0 0 0 | z z z z
|
|
// 0 1 0 0 | z z z z
|
|
// 0 0 1 0 | z z z z
|
|
// 0 0 0 1 | z z z z
|
|
//
|
|
|
|
real mij;
|
|
for(i=3;i>0;i--)
|
|
{
|
|
for(j=i-1;j > -1; j--)
|
|
{
|
|
mij = s[j][i]/s[i][i];
|
|
for(jj=j+1;jj<8;jj++)
|
|
s[j][jj] -= mij*s[i][jj];
|
|
}
|
|
}
|
|
|
|
for(i=0;i<4;i++)
|
|
for(j=0;j<4;j++)
|
|
minv(i,j) = s[i][j+4] / s[i][i];
|
|
|
|
return minv;
|
|
}
|
|
|
|
|
|
matrix4 transpose() const
|
|
{
|
|
matrix4 mtrans;
|
|
|
|
for(int i=0;i<4;i++)
|
|
for(int j=0;j<4;j++)
|
|
mtrans(i,j) = element(j,i);
|
|
return mtrans;
|
|
}
|
|
|
|
matrix4 & mult_right( const matrix4 & b )
|
|
{
|
|
matrix4 mt(*this);
|
|
set_value(real(0));
|
|
|
|
for(int i=0; i < 4; i++)
|
|
for(int j=0; j < 4; j++)
|
|
for(int c=0; c < 4; c++)
|
|
element(i,j) += mt(i,c) * b(c,j);
|
|
return *this;
|
|
}
|
|
|
|
matrix4 & mult_left( const matrix4 & b )
|
|
{
|
|
matrix4 mt(*this);
|
|
set_value(real(0));
|
|
|
|
for(int i=0; i < 4; i++)
|
|
for(int j=0; j < 4; j++)
|
|
for(int c=0; c < 4; c++)
|
|
element(i,j) += b(i,c) * mt(c,j);
|
|
return *this;
|
|
}
|
|
|
|
// dst = M * src
|
|
void mult_matrix_vec( const vec3 &src, vec3 &dst ) const
|
|
{
|
|
real w = (
|
|
src.v[0] * element(3,0) +
|
|
src.v[1] * element(3,1) +
|
|
src.v[2] * element(3,2) +
|
|
element(3,3) );
|
|
|
|
assert(w != GLH_ZERO);
|
|
|
|
dst.v[0] = (
|
|
src.v[0] * element(0,0) +
|
|
src.v[1] * element(0,1) +
|
|
src.v[2] * element(0,2) +
|
|
element(0,3) ) / w;
|
|
dst.v[1] = (
|
|
src.v[0] * element(1,0) +
|
|
src.v[1] * element(1,1) +
|
|
src.v[2] * element(1,2) +
|
|
element(1,3) ) / w;
|
|
dst.v[2] = (
|
|
src.v[0] * element(2,0) +
|
|
src.v[1] * element(2,1) +
|
|
src.v[2] * element(2,2) +
|
|
element(2,3) ) / w;
|
|
}
|
|
|
|
void mult_matrix_vec( vec3 & src_and_dst) const
|
|
{ mult_matrix_vec(vec3(src_and_dst), src_and_dst); }
|
|
|
|
|
|
// dst = src * M
|
|
void mult_vec_matrix( const vec3 &src, vec3 &dst ) const
|
|
{
|
|
real w = (
|
|
src.v[0] * element(0,3) +
|
|
src.v[1] * element(1,3) +
|
|
src.v[2] * element(2,3) +
|
|
element(3,3) );
|
|
|
|
assert(w != GLH_ZERO);
|
|
|
|
dst.v[0] = (
|
|
src.v[0] * element(0,0) +
|
|
src.v[1] * element(1,0) +
|
|
src.v[2] * element(2,0) +
|
|
element(3,0) ) / w;
|
|
dst.v[1] = (
|
|
src.v[0] * element(0,1) +
|
|
src.v[1] * element(1,1) +
|
|
src.v[2] * element(2,1) +
|
|
element(3,1) ) / w;
|
|
dst.v[2] = (
|
|
src.v[0] * element(0,2) +
|
|
src.v[1] * element(1,2) +
|
|
src.v[2] * element(2,2) +
|
|
element(3,2) ) / w;
|
|
}
|
|
|
|
|
|
void mult_vec_matrix( vec3 & src_and_dst) const
|
|
{ mult_vec_matrix(vec3(src_and_dst), src_and_dst); }
|
|
|
|
// dst = M * src
|
|
void mult_matrix_vec( const vec4 &src, vec4 &dst ) const
|
|
{
|
|
dst.v[0] = (
|
|
src.v[0] * element(0,0) +
|
|
src.v[1] * element(0,1) +
|
|
src.v[2] * element(0,2) +
|
|
src.v[3] * element(0,3));
|
|
dst.v[1] = (
|
|
src.v[0] * element(1,0) +
|
|
src.v[1] * element(1,1) +
|
|
src.v[2] * element(1,2) +
|
|
src.v[3] * element(1,3));
|
|
dst.v[2] = (
|
|
src.v[0] * element(2,0) +
|
|
src.v[1] * element(2,1) +
|
|
src.v[2] * element(2,2) +
|
|
src.v[3] * element(2,3));
|
|
dst.v[3] = (
|
|
src.v[0] * element(3,0) +
|
|
src.v[1] * element(3,1) +
|
|
src.v[2] * element(3,2) +
|
|
src.v[3] * element(3,3));
|
|
}
|
|
|
|
void mult_matrix_vec( vec4 & src_and_dst) const
|
|
{ mult_matrix_vec(vec4(src_and_dst), src_and_dst); }
|
|
|
|
|
|
// dst = src * M
|
|
void mult_vec_matrix( const vec4 &src, vec4 &dst ) const
|
|
{
|
|
dst.v[0] = (
|
|
src.v[0] * element(0,0) +
|
|
src.v[1] * element(1,0) +
|
|
src.v[2] * element(2,0) +
|
|
src.v[3] * element(3,0));
|
|
dst.v[1] = (
|
|
src.v[0] * element(0,1) +
|
|
src.v[1] * element(1,1) +
|
|
src.v[2] * element(2,1) +
|
|
src.v[3] * element(3,1));
|
|
dst.v[2] = (
|
|
src.v[0] * element(0,2) +
|
|
src.v[1] * element(1,2) +
|
|
src.v[2] * element(2,2) +
|
|
src.v[3] * element(3,2));
|
|
dst.v[3] = (
|
|
src.v[0] * element(0,3) +
|
|
src.v[1] * element(1,3) +
|
|
src.v[2] * element(2,3) +
|
|
src.v[3] * element(3,3));
|
|
}
|
|
|
|
|
|
void mult_vec_matrix( vec4 & src_and_dst) const
|
|
{ mult_vec_matrix(vec4(src_and_dst), src_and_dst); }
|
|
|
|
|
|
// dst = M * src
|
|
void mult_matrix_dir( const vec3 &src, vec3 &dst ) const
|
|
{
|
|
dst.v[0] = (
|
|
src.v[0] * element(0,0) +
|
|
src.v[1] * element(0,1) +
|
|
src.v[2] * element(0,2) ) ;
|
|
dst.v[1] = (
|
|
src.v[0] * element(1,0) +
|
|
src.v[1] * element(1,1) +
|
|
src.v[2] * element(1,2) ) ;
|
|
dst.v[2] = (
|
|
src.v[0] * element(2,0) +
|
|
src.v[1] * element(2,1) +
|
|
src.v[2] * element(2,2) ) ;
|
|
}
|
|
|
|
|
|
void mult_matrix_dir( vec3 & src_and_dst) const
|
|
{ mult_matrix_dir(vec3(src_and_dst), src_and_dst); }
|
|
|
|
|
|
// dst = src * M
|
|
void mult_dir_matrix( const vec3 &src, vec3 &dst ) const
|
|
{
|
|
dst.v[0] = (
|
|
src.v[0] * element(0,0) +
|
|
src.v[1] * element(1,0) +
|
|
src.v[2] * element(2,0) ) ;
|
|
dst.v[1] = (
|
|
src.v[0] * element(0,1) +
|
|
src.v[1] * element(1,1) +
|
|
src.v[2] * element(2,1) ) ;
|
|
dst.v[2] = (
|
|
src.v[0] * element(0,2) +
|
|
src.v[1] * element(1,2) +
|
|
src.v[2] * element(2,2) ) ;
|
|
}
|
|
|
|
|
|
void mult_dir_matrix( vec3 & src_and_dst) const
|
|
{ mult_dir_matrix(vec3(src_and_dst), src_and_dst); }
|
|
|
|
|
|
real & operator () (int row, int col)
|
|
{ return element(row,col); }
|
|
|
|
const real & operator () (int row, int col) const
|
|
{ return element(row,col); }
|
|
|
|
real & element (int row, int col)
|
|
{ return m[row | (col<<2)]; }
|
|
|
|
const real & element (int row, int col) const
|
|
{ return m[row | (col<<2)]; }
|
|
|
|
matrix4 & operator *= ( const matrix4 & mat )
|
|
{
|
|
mult_right( mat );
|
|
return *this;
|
|
}
|
|
|
|
matrix4 & operator *= ( const real & r )
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
element(0,i) *= r;
|
|
element(1,i) *= r;
|
|
element(2,i) *= r;
|
|
element(3,i) *= r;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
matrix4 & operator += ( const matrix4 & mat )
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
element(0,i) += mat.element(0,i);
|
|
element(1,i) += mat.element(1,i);
|
|
element(2,i) += mat.element(2,i);
|
|
element(3,i) += mat.element(3,i);
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
friend matrix4 operator * ( const matrix4 & m1, const matrix4 & m2 );
|
|
friend bool operator == ( const matrix4 & m1, const matrix4 & m2 );
|
|
friend bool operator != ( const matrix4 & m1, const matrix4 & m2 );
|
|
|
|
//protected:
|
|
real m[16];
|
|
};
|
|
|
|
inline
|
|
matrix4 operator * ( const matrix4 & m1, const matrix4 & m2 )
|
|
{
|
|
matrix4 product;
|
|
|
|
product = m1;
|
|
product.mult_right(m2);
|
|
|
|
return product;
|
|
}
|
|
|
|
inline
|
|
bool operator ==( const matrix4 &m1, const matrix4 &m2 )
|
|
{
|
|
return (
|
|
m1(0,0) == m2(0,0) &&
|
|
m1(0,1) == m2(0,1) &&
|
|
m1(0,2) == m2(0,2) &&
|
|
m1(0,3) == m2(0,3) &&
|
|
m1(1,0) == m2(1,0) &&
|
|
m1(1,1) == m2(1,1) &&
|
|
m1(1,2) == m2(1,2) &&
|
|
m1(1,3) == m2(1,3) &&
|
|
m1(2,0) == m2(2,0) &&
|
|
m1(2,1) == m2(2,1) &&
|
|
m1(2,2) == m2(2,2) &&
|
|
m1(2,3) == m2(2,3) &&
|
|
m1(3,0) == m2(3,0) &&
|
|
m1(3,1) == m2(3,1) &&
|
|
m1(3,2) == m2(3,2) &&
|
|
m1(3,3) == m2(3,3) );
|
|
}
|
|
|
|
inline
|
|
bool operator != ( const matrix4 & m1, const matrix4 & m2 )
|
|
{ return !( m1 == m2 ); }
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class quaternion
|
|
{
|
|
public:
|
|
|
|
quaternion()
|
|
{
|
|
*this = identity();
|
|
}
|
|
|
|
quaternion( const real v[4] )
|
|
{
|
|
set_value( v );
|
|
}
|
|
|
|
|
|
quaternion( real q0, real q1, real q2, real q3 )
|
|
{
|
|
set_value( q0, q1, q2, q3 );
|
|
}
|
|
|
|
|
|
quaternion( const matrix4 & m )
|
|
{
|
|
set_value( m );
|
|
}
|
|
|
|
|
|
quaternion( const vec3 &axis, real radians )
|
|
{
|
|
set_value( axis, radians );
|
|
}
|
|
|
|
|
|
quaternion( const vec3 &rotateFrom, const vec3 &rotateTo )
|
|
{
|
|
set_value( rotateFrom, rotateTo );
|
|
}
|
|
|
|
quaternion( const vec3 & from_look, const vec3 & from_up,
|
|
const vec3 & to_look, const vec3& to_up)
|
|
{
|
|
set_value(from_look, from_up, to_look, to_up);
|
|
}
|
|
|
|
const real * get_value() const
|
|
{
|
|
return &q[0];
|
|
}
|
|
|
|
void get_value( real &q0, real &q1, real &q2, real &q3 ) const
|
|
{
|
|
q0 = q[0];
|
|
q1 = q[1];
|
|
q2 = q[2];
|
|
q3 = q[3];
|
|
}
|
|
|
|
quaternion & set_value( real q0, real q1, real q2, real q3 )
|
|
{
|
|
q[0] = q0;
|
|
q[1] = q1;
|
|
q[2] = q2;
|
|
q[3] = q3;
|
|
counter = 0;
|
|
return *this;
|
|
}
|
|
|
|
void get_value( vec3 &axis, real &radians ) const
|
|
{
|
|
radians = real(acos( q[3] ) * GLH_TWO);
|
|
if ( radians == GLH_ZERO )
|
|
axis = vec3( 0.0, 0.0, 1.0 );
|
|
else
|
|
{
|
|
axis.v[0] = q[0];
|
|
axis.v[1] = q[1];
|
|
axis.v[2] = q[2];
|
|
axis.normalize();
|
|
}
|
|
}
|
|
|
|
void get_value( matrix4 & m ) const
|
|
{
|
|
real s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
|
|
|
|
real norm = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
|
|
|
|
s = (equivalent(norm,GLH_ZERO)) ? GLH_ZERO : ( GLH_TWO / norm );
|
|
|
|
xs = q[0] * s;
|
|
ys = q[1] * s;
|
|
zs = q[2] * s;
|
|
|
|
wx = q[3] * xs;
|
|
wy = q[3] * ys;
|
|
wz = q[3] * zs;
|
|
|
|
xx = q[0] * xs;
|
|
xy = q[0] * ys;
|
|
xz = q[0] * zs;
|
|
|
|
yy = q[1] * ys;
|
|
yz = q[1] * zs;
|
|
zz = q[2] * zs;
|
|
|
|
m(0,0) = real( GLH_ONE - ( yy + zz ));
|
|
m(1,0) = real ( xy + wz );
|
|
m(2,0) = real ( xz - wy );
|
|
|
|
m(0,1) = real ( xy - wz );
|
|
m(1,1) = real ( GLH_ONE - ( xx + zz ));
|
|
m(2,1) = real ( yz + wx );
|
|
|
|
m(0,2) = real ( xz + wy );
|
|
m(1,2) = real ( yz - wx );
|
|
m(2,2) = real ( GLH_ONE - ( xx + yy ));
|
|
|
|
m(3,0) = m(3,1) = m(3,2) = m(0,3) = m(1,3) = m(2,3) = GLH_ZERO;
|
|
m(3,3) = GLH_ONE;
|
|
}
|
|
|
|
quaternion & set_value( const real * qp )
|
|
{
|
|
memcpy(q,qp,sizeof(real) * 4);
|
|
|
|
counter = 0;
|
|
return *this;
|
|
}
|
|
|
|
quaternion & set_value( const matrix4 & m )
|
|
{
|
|
real tr, s;
|
|
int i, j, k;
|
|
const int nxt[3] = { 1, 2, 0 };
|
|
|
|
tr = m(0,0) + m(1,1) + m(2,2);
|
|
|
|
if ( tr > GLH_ZERO )
|
|
{
|
|
s = real(sqrt( tr + m(3,3) ));
|
|
q[3] = real ( s * 0.5 );
|
|
s = real(0.5) / s;
|
|
|
|
q[0] = real ( ( m(1,2) - m(2,1) ) * s );
|
|
q[1] = real ( ( m(2,0) - m(0,2) ) * s );
|
|
q[2] = real ( ( m(0,1) - m(1,0) ) * s );
|
|
}
|
|
else
|
|
{
|
|
i = 0;
|
|
if ( m(1,1) > m(0,0) )
|
|
i = 1;
|
|
|
|
if ( m(2,2) > m(i,i) )
|
|
i = 2;
|
|
|
|
j = nxt[i];
|
|
k = nxt[j];
|
|
|
|
s = real(sqrt( ( m(i,j) - ( m(j,j) + m(k,k) )) + GLH_ONE ));
|
|
|
|
q[i] = real ( s * 0.5 );
|
|
s = real(0.5 / s);
|
|
|
|
q[3] = real ( ( m(j,k) - m(k,j) ) * s );
|
|
q[j] = real ( ( m(i,j) + m(j,i) ) * s );
|
|
q[k] = real ( ( m(i,k) + m(k,i) ) * s );
|
|
}
|
|
|
|
counter = 0;
|
|
return *this;
|
|
}
|
|
|
|
quaternion & set_value( const vec3 &axis, real theta )
|
|
{
|
|
real sqnorm = axis.square_norm();
|
|
|
|
if (sqnorm <= GLH_EPSILON)
|
|
{
|
|
// axis too small.
|
|
x = y = z = 0.0;
|
|
w = 1.0;
|
|
}
|
|
else
|
|
{
|
|
theta *= real(0.5);
|
|
real sin_theta = real(sin(theta));
|
|
|
|
if (!equivalent(sqnorm,GLH_ONE))
|
|
sin_theta /= real(sqrt(sqnorm));
|
|
x = sin_theta * axis.v[0];
|
|
y = sin_theta * axis.v[1];
|
|
z = sin_theta * axis.v[2];
|
|
w = real(cos(theta));
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
quaternion & set_value( const vec3 & rotateFrom, const vec3 & rotateTo )
|
|
{
|
|
vec3 p1, p2;
|
|
real alpha;
|
|
|
|
p1 = rotateFrom;
|
|
p1.normalize();
|
|
p2 = rotateTo;
|
|
p2.normalize();
|
|
|
|
alpha = p1.dot(p2);
|
|
|
|
if(equivalent(alpha,GLH_ONE))
|
|
{
|
|
*this = identity();
|
|
return *this;
|
|
}
|
|
|
|
// ensures that the anti-parallel case leads to a positive dot
|
|
if(equivalent(alpha,-GLH_ONE))
|
|
{
|
|
vec3 v;
|
|
|
|
if(p1.v[0] != p1.v[1] || p1.v[0] != p1.v[2])
|
|
v = vec3(p1.v[1], p1.v[2], p1.v[0]);
|
|
else
|
|
v = vec3(-p1.v[0], p1.v[1], p1.v[2]);
|
|
|
|
v -= p1 * p1.dot(v);
|
|
v.normalize();
|
|
|
|
set_value(v, GLH_PI);
|
|
return *this;
|
|
}
|
|
|
|
p1 = p1.cross(p2);
|
|
p1.normalize();
|
|
set_value(p1,real(acos(alpha)));
|
|
|
|
counter = 0;
|
|
return *this;
|
|
}
|
|
|
|
quaternion & set_value( const vec3 & from_look, const vec3 & from_up,
|
|
const vec3 & to_look, const vec3 & to_up)
|
|
{
|
|
quaternion r_look = quaternion(from_look, to_look);
|
|
|
|
vec3 rotated_from_up(from_up);
|
|
r_look.mult_vec(rotated_from_up);
|
|
|
|
quaternion r_twist = quaternion(rotated_from_up, to_up);
|
|
|
|
*this = r_twist;
|
|
*this *= r_look;
|
|
return *this;
|
|
}
|
|
|
|
quaternion & operator *= ( const quaternion & qr )
|
|
{
|
|
quaternion ql(*this);
|
|
|
|
w = ql.w * qr.w - ql.x * qr.x - ql.y * qr.y - ql.z * qr.z;
|
|
x = ql.w * qr.x + ql.x * qr.w + ql.y * qr.z - ql.z * qr.y;
|
|
y = ql.w * qr.y + ql.y * qr.w + ql.z * qr.x - ql.x * qr.z;
|
|
z = ql.w * qr.z + ql.z * qr.w + ql.x * qr.y - ql.y * qr.x;
|
|
|
|
counter += qr.counter;
|
|
counter++;
|
|
counter_normalize();
|
|
return *this;
|
|
}
|
|
|
|
void normalize()
|
|
{
|
|
real rnorm = GLH_ONE / real(sqrt(w * w + x * x + y * y + z * z));
|
|
if (equivalent(rnorm, GLH_ZERO))
|
|
return;
|
|
x *= rnorm;
|
|
y *= rnorm;
|
|
z *= rnorm;
|
|
w *= rnorm;
|
|
counter = 0;
|
|
}
|
|
|
|
friend bool operator == ( const quaternion & q1, const quaternion & q2 );
|
|
|
|
friend bool operator != ( const quaternion & q1, const quaternion & q2 );
|
|
|
|
friend quaternion operator * ( const quaternion & q1, const quaternion & q2 );
|
|
|
|
bool equals( const quaternion & r, real tolerance ) const
|
|
{
|
|
real t;
|
|
|
|
t = (
|
|
(q[0]-r.q[0])*(q[0]-r.q[0]) +
|
|
(q[1]-r.q[1])*(q[1]-r.q[1]) +
|
|
(q[2]-r.q[2])*(q[2]-r.q[2]) +
|
|
(q[3]-r.q[3])*(q[3]-r.q[3]) );
|
|
if(t > GLH_EPSILON)
|
|
return false;
|
|
return 1;
|
|
}
|
|
|
|
quaternion & conjugate()
|
|
{
|
|
q[0] *= -GLH_ONE;
|
|
q[1] *= -GLH_ONE;
|
|
q[2] *= -GLH_ONE;
|
|
return *this;
|
|
}
|
|
|
|
quaternion & invert()
|
|
{
|
|
return conjugate();
|
|
}
|
|
|
|
quaternion inverse() const
|
|
{
|
|
quaternion r = *this;
|
|
return r.invert();
|
|
}
|
|
|
|
//
|
|
// Quaternion multiplication with cartesian vector
|
|
// v' = q*v*q(star)
|
|
//
|
|
void mult_vec( const vec3 &src, vec3 &dst ) const
|
|
{
|
|
real v_coef = w * w - x * x - y * y - z * z;
|
|
real u_coef = GLH_TWO * (src.v[0] * x + src.v[1] * y + src.v[2] * z);
|
|
real c_coef = GLH_TWO * w;
|
|
|
|
dst.v[0] = v_coef * src.v[0] + u_coef * x + c_coef * (y * src.v[2] - z * src.v[1]);
|
|
dst.v[1] = v_coef * src.v[1] + u_coef * y + c_coef * (z * src.v[0] - x * src.v[2]);
|
|
dst.v[2] = v_coef * src.v[2] + u_coef * z + c_coef * (x * src.v[1] - y * src.v[0]);
|
|
}
|
|
|
|
void mult_vec( vec3 & src_and_dst) const
|
|
{
|
|
mult_vec(vec3(src_and_dst), src_and_dst);
|
|
}
|
|
|
|
void scale_angle( real scaleFactor )
|
|
{
|
|
vec3 axis;
|
|
real radians;
|
|
|
|
get_value(axis, radians);
|
|
radians *= scaleFactor;
|
|
set_value(axis, radians);
|
|
}
|
|
|
|
static quaternion slerp( const quaternion & p, const quaternion & q, real alpha )
|
|
{
|
|
quaternion r;
|
|
|
|
real cos_omega = p.x * q.x + p.y * q.y + p.z * q.z + p.w * q.w;
|
|
// if B is on opposite hemisphere from A, use -B instead
|
|
|
|
int bflip;
|
|
if ( ( bflip = (cos_omega < GLH_ZERO)) )
|
|
cos_omega = -cos_omega;
|
|
|
|
// complementary interpolation parameter
|
|
real beta = GLH_ONE - alpha;
|
|
|
|
if(cos_omega <= GLH_ONE - GLH_EPSILON)
|
|
return p;
|
|
|
|
real omega = real(acos(cos_omega));
|
|
real one_over_sin_omega = GLH_ONE / real(sin(omega));
|
|
|
|
beta = real(sin(omega*beta) * one_over_sin_omega);
|
|
alpha = real(sin(omega*alpha) * one_over_sin_omega);
|
|
|
|
if (bflip)
|
|
alpha = -alpha;
|
|
|
|
r.x = beta * p.q[0]+ alpha * q.q[0];
|
|
r.y = beta * p.q[1]+ alpha * q.q[1];
|
|
r.z = beta * p.q[2]+ alpha * q.q[2];
|
|
r.w = beta * p.q[3]+ alpha * q.q[3];
|
|
return r;
|
|
}
|
|
|
|
static quaternion identity()
|
|
{
|
|
static quaternion ident( vec3( 0.0, 0.0, 0.0 ), GLH_ONE );
|
|
return ident;
|
|
}
|
|
|
|
real & operator []( int i )
|
|
{
|
|
assert(i < 4);
|
|
return q[i];
|
|
}
|
|
|
|
const real & operator []( int i ) const
|
|
{
|
|
assert(i < 4);
|
|
return q[i];
|
|
}
|
|
|
|
protected:
|
|
|
|
void counter_normalize()
|
|
{
|
|
if (counter > GLH_QUATERNION_NORMALIZATION_THRESHOLD)
|
|
normalize();
|
|
}
|
|
|
|
union
|
|
{
|
|
struct
|
|
{
|
|
real q[4];
|
|
};
|
|
struct
|
|
{
|
|
real x;
|
|
real y;
|
|
real z;
|
|
real w;
|
|
};
|
|
};
|
|
|
|
// renormalization counter
|
|
unsigned char counter;
|
|
};
|
|
|
|
inline
|
|
bool operator == ( const quaternion & q1, const quaternion & q2 )
|
|
{
|
|
return (equivalent(q1.x, q2.x) &&
|
|
equivalent(q1.y, q2.y) &&
|
|
equivalent(q1.z, q2.z) &&
|
|
equivalent(q1.w, q2.w) );
|
|
}
|
|
|
|
inline
|
|
bool operator != ( const quaternion & q1, const quaternion & q2 )
|
|
{
|
|
return ! ( q1 == q2 );
|
|
}
|
|
|
|
inline
|
|
quaternion operator * ( const quaternion & q1, const quaternion & q2 )
|
|
{
|
|
quaternion r(q1);
|
|
r *= q2;
|
|
return r;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class plane
|
|
{
|
|
public:
|
|
|
|
plane()
|
|
{
|
|
planedistance = 0.0;
|
|
planenormal.set_value( 0.0, 0.0, 1.0 );
|
|
}
|
|
|
|
|
|
plane( const vec3 &p0, const vec3 &p1, const vec3 &p2 )
|
|
{
|
|
vec3 v0 = p1 - p0;
|
|
vec3 v1 = p2 - p0;
|
|
planenormal = v0.cross(v1);
|
|
planenormal.normalize();
|
|
planedistance = p0.dot(planenormal);
|
|
}
|
|
|
|
plane( const vec3 &normal, real distance )
|
|
{
|
|
planedistance = distance;
|
|
planenormal = normal;
|
|
planenormal.normalize();
|
|
}
|
|
|
|
plane( const vec3 &normal, const vec3 &point )
|
|
{
|
|
planenormal = normal;
|
|
planenormal.normalize();
|
|
planedistance = point.dot(planenormal);
|
|
}
|
|
|
|
void offset( real d )
|
|
{
|
|
planedistance += d;
|
|
}
|
|
|
|
bool intersect( const line &l, vec3 &intersection ) const
|
|
{
|
|
vec3 pos, dir;
|
|
vec3 pn = planenormal;
|
|
real pd = planedistance;
|
|
|
|
pos = l.get_position();
|
|
dir = l.get_direction();
|
|
|
|
if(dir.dot(pn) == 0.0) return 0;
|
|
pos -= pn*pd;
|
|
// now we're talking about a plane passing through the origin
|
|
if(pos.dot(pn) < 0.0) pn.negate();
|
|
if(dir.dot(pn) > 0.0) dir.negate();
|
|
vec3 ppos = pn * pos.dot(pn);
|
|
pos = (ppos.length()/dir.dot(-pn))*dir;
|
|
intersection = l.get_position();
|
|
intersection += pos;
|
|
return 1;
|
|
}
|
|
void transform( const matrix4 &matrix )
|
|
{
|
|
matrix4 invtr = matrix.inverse();
|
|
invtr = invtr.transpose();
|
|
|
|
vec3 pntOnplane = planenormal * planedistance;
|
|
vec3 newPntOnplane;
|
|
vec3 newnormal;
|
|
|
|
invtr.mult_dir_matrix(planenormal, newnormal);
|
|
matrix.mult_vec_matrix(pntOnplane, newPntOnplane);
|
|
|
|
newnormal.normalize();
|
|
planenormal = newnormal;
|
|
planedistance = newPntOnplane.dot(planenormal);
|
|
}
|
|
|
|
bool is_in_half_space( const vec3 &point ) const
|
|
{
|
|
|
|
if(( point.dot(planenormal) - planedistance) < 0.0)
|
|
return 0;
|
|
return 1;
|
|
}
|
|
|
|
|
|
real distance( const vec3 & point ) const
|
|
{
|
|
return planenormal.dot(point - planenormal*planedistance);
|
|
}
|
|
|
|
const vec3 &get_normal() const
|
|
{
|
|
return planenormal;
|
|
}
|
|
|
|
|
|
real get_distance_from_origin() const
|
|
{
|
|
return planedistance;
|
|
}
|
|
|
|
|
|
friend bool operator == ( const plane & p1, const plane & p2 );
|
|
|
|
|
|
friend bool operator != ( const plane & p1, const plane & p2 );
|
|
|
|
//protected:
|
|
vec3 planenormal;
|
|
real planedistance;
|
|
};
|
|
|
|
inline
|
|
bool operator == (const plane & p1, const plane & p2 )
|
|
{
|
|
return ( p1.planedistance == p2.planedistance && p1.planenormal == p2.planenormal);
|
|
}
|
|
|
|
inline
|
|
bool operator != ( const plane & p1, const plane & p2 )
|
|
{ return ! (p1 == p2); }
|
|
|
|
|
|
|
|
} // "ns_##GLH_REAL"
|
|
|
|
// make common typedefs...
|
|
#ifdef GLH_REAL_IS_FLOAT
|
|
typedef GLH_REAL_NAMESPACE::vec2 vec2f;
|
|
typedef GLH_REAL_NAMESPACE::vec3 vec3f;
|
|
typedef GLH_REAL_NAMESPACE::vec4 vec4f;
|
|
typedef GLH_REAL_NAMESPACE::quaternion quaternionf;
|
|
typedef GLH_REAL_NAMESPACE::quaternion rotationf;
|
|
typedef GLH_REAL_NAMESPACE::line linef;
|
|
typedef GLH_REAL_NAMESPACE::plane planef;
|
|
typedef GLH_REAL_NAMESPACE::matrix4 matrix4f;
|
|
#endif
|
|
|
|
|
|
|
|
|
|
} // namespace glh
|
|
|
|
|
|
|
|
#endif
|
|
|