[845] | 1 | /*
|
---|
| 2 | glh - is a platform-indepenedent C++ OpenGL helper library
|
---|
| 3 |
|
---|
| 4 |
|
---|
| 5 | Copyright (c) 2000 Cass Everitt
|
---|
| 6 | Copyright (c) 2000 NVIDIA Corporation
|
---|
| 7 | All rights reserved.
|
---|
| 8 |
|
---|
| 9 | Redistribution and use in source and binary forms, with or
|
---|
| 10 | without modification, are permitted provided that the following
|
---|
| 11 | conditions are met:
|
---|
| 12 |
|
---|
| 13 | * Redistributions of source code must retain the above
|
---|
| 14 | copyright notice, this list of conditions and the following
|
---|
| 15 | disclaimer.
|
---|
| 16 |
|
---|
| 17 | * Redistributions in binary form must reproduce the above
|
---|
| 18 | copyright notice, this list of conditions and the following
|
---|
| 19 | disclaimer in the documentation and/or other materials
|
---|
| 20 | provided with the distribution.
|
---|
| 21 |
|
---|
| 22 | * The names of contributors to this software may not be used
|
---|
| 23 | to endorse or promote products derived from this software
|
---|
| 24 | without specific prior written permission.
|
---|
| 25 |
|
---|
| 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
---|
| 27 | ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
---|
| 28 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
---|
| 29 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
---|
| 30 | REGENTS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
---|
| 31 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
---|
| 32 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
---|
| 33 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
---|
| 34 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
---|
| 35 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
---|
| 36 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
---|
| 37 | POSSIBILITY OF SUCH DAMAGE.
|
---|
| 38 |
|
---|
| 39 |
|
---|
| 40 | Cass Everitt - cass@r3.nu
|
---|
| 41 | */
|
---|
| 42 |
|
---|
| 43 | /*
|
---|
| 44 | glh_linear.h
|
---|
| 45 | */
|
---|
| 46 |
|
---|
| 47 | // Author: Cass W. Everitt
|
---|
| 48 |
|
---|
| 49 | #ifndef GLH_LINEAR_H
|
---|
| 50 | #define GLH_LINEAR_H
|
---|
| 51 |
|
---|
| 52 | #include <memory.h>
|
---|
| 53 | #include <math.h>
|
---|
| 54 | #include <assert.h>
|
---|
| 55 |
|
---|
| 56 | // only supports float for now...
|
---|
| 57 | #define GLH_REAL_IS_FLOAT
|
---|
| 58 |
|
---|
| 59 | #ifdef GLH_REAL_IS_FLOAT
|
---|
| 60 | # define GLH_REAL float
|
---|
| 61 | # define GLH_REAL_NAMESPACE ns_float
|
---|
| 62 | #endif
|
---|
| 63 |
|
---|
| 64 | #ifdef _WIN32
|
---|
| 65 | # define TEMPLATE_FUNCTION
|
---|
| 66 | #else
|
---|
| 67 | # define TEMPLATE_FUNCTION <>
|
---|
| 68 | #endif
|
---|
| 69 |
|
---|
| 70 | #define GLH_QUATERNION_NORMALIZATION_THRESHOLD 64
|
---|
| 71 |
|
---|
| 72 | #define GLH_RAD_TO_DEG GLH_REAL(57.2957795130823208767981548141052)
|
---|
| 73 | #define GLH_DEG_TO_RAD GLH_REAL(0.0174532925199432957692369076848861)
|
---|
| 74 | #define GLH_ZERO GLH_REAL(0.0)
|
---|
| 75 | #define GLH_ONE GLH_REAL(1.0)
|
---|
| 76 | #define GLH_TWO GLH_REAL(2.0)
|
---|
| 77 | #define GLH_EPSILON GLH_REAL(10e-6)
|
---|
| 78 | #define GLH_PI GLH_REAL(3.1415926535897932384626433832795)
|
---|
| 79 |
|
---|
| 80 | #define equivalent(a,b) (((a < b + GLH_EPSILON) && (a > b - GLH_EPSILON)) ? true : false)
|
---|
| 81 |
|
---|
| 82 | namespace glh
|
---|
| 83 | {
|
---|
| 84 |
|
---|
| 85 | inline GLH_REAL to_degrees(GLH_REAL radians) { return radians*GLH_RAD_TO_DEG; }
|
---|
| 86 | inline GLH_REAL to_radians(GLH_REAL degrees) { return degrees*GLH_DEG_TO_RAD; }
|
---|
| 87 |
|
---|
| 88 |
|
---|
| 89 | template <int N, class T>
|
---|
| 90 | class vec
|
---|
| 91 | {
|
---|
| 92 | public:
|
---|
| 93 | int size() const { return N; }
|
---|
| 94 |
|
---|
| 95 | vec(const T & t = T())
|
---|
| 96 | { for(int i = 0; i < N; i++) v[i] = t; }
|
---|
| 97 | vec(const T * tp)
|
---|
| 98 | { for(int i = 0; i < N; i++) v[i] = tp[i]; }
|
---|
| 99 |
|
---|
| 100 | const T * get_value() const
|
---|
| 101 | { return v; }
|
---|
| 102 |
|
---|
| 103 |
|
---|
| 104 | T dot( const vec<N,T> & rhs ) const
|
---|
| 105 | {
|
---|
| 106 | T r = 0;
|
---|
| 107 | for(int i = 0; i < N; i++) r += v[i]*rhs.v[i];
|
---|
| 108 | return r;
|
---|
| 109 | }
|
---|
| 110 |
|
---|
| 111 | T length() const
|
---|
| 112 | {
|
---|
| 113 | T r = 0;
|
---|
| 114 | for(int i = 0; i < N; i++) r += v[i]*v[i];
|
---|
| 115 | return T(sqrt(r));
|
---|
| 116 | }
|
---|
| 117 |
|
---|
| 118 | T square_norm() const
|
---|
| 119 | {
|
---|
| 120 | T r = 0;
|
---|
| 121 | for(int i = 0; i < N; i++) r += v[i]*v[i];
|
---|
| 122 | return r;
|
---|
| 123 | }
|
---|
| 124 |
|
---|
| 125 | void negate()
|
---|
| 126 | { for(int i = 0; i < N; i++) v[i] = -v[i]; }
|
---|
| 127 |
|
---|
| 128 |
|
---|
| 129 | T normalize()
|
---|
| 130 | {
|
---|
| 131 | T sum(0);
|
---|
| 132 | for(int i = 0; i < N; i++)
|
---|
| 133 | sum += v[i]*v[i];
|
---|
| 134 | sum = T(sqrt(sum));
|
---|
| 135 | if (sum > GLH_EPSILON)
|
---|
| 136 | for(int i = 0; i < N; i++)
|
---|
| 137 | v[i] /= sum;
|
---|
| 138 | return sum;
|
---|
| 139 | }
|
---|
| 140 |
|
---|
| 141 |
|
---|
| 142 | vec<N,T> & set_value( const T * rhs )
|
---|
| 143 | { for(int i = 0; i < N; i++) v[i] = rhs[i]; return *this; }
|
---|
| 144 |
|
---|
| 145 | T & operator [] ( int i )
|
---|
| 146 | { return v[i]; }
|
---|
| 147 |
|
---|
| 148 | const T & operator [] ( int i ) const
|
---|
| 149 | { return v[i]; }
|
---|
| 150 |
|
---|
| 151 | vec<N,T> & operator *= ( T d )
|
---|
| 152 | { for(int i = 0; i < N; i++) v[i] *= d; return *this;}
|
---|
| 153 |
|
---|
| 154 | vec<N,T> & operator *= ( const vec<N,T> & u )
|
---|
| 155 | { for(int i = 0; i < N; i++) v[i] *= u[i]; return *this;}
|
---|
| 156 |
|
---|
| 157 | vec<N,T> & operator /= ( T d )
|
---|
| 158 | { if(d == 0) return *this; for(int i = 0; i < N; i++) v[i] /= d; return *this;}
|
---|
| 159 |
|
---|
| 160 | vec<N,T> & operator += ( const vec<N,T> & u )
|
---|
| 161 | { for(int i = 0; i < N; i++) v[i] += u.v[i]; return *this;}
|
---|
| 162 |
|
---|
| 163 | vec<N,T> & operator -= ( const vec<N,T> & u )
|
---|
| 164 | { for(int i = 0; i < N; i++) v[i] -= u.v[i]; return *this;}
|
---|
| 165 |
|
---|
| 166 |
|
---|
| 167 | vec<N,T> operator - () const
|
---|
| 168 | { vec<N,T> rv = v; rv.negate(); return rv; }
|
---|
| 169 |
|
---|
| 170 | vec<N,T> operator + ( const vec<N,T> &v) const
|
---|
| 171 | { vec<N,T> rt(*this); return rt += v; }
|
---|
| 172 |
|
---|
| 173 | vec<N,T> operator - ( const vec<N,T> &v) const
|
---|
| 174 | { vec<N,T> rt(*this); return rt -= v; }
|
---|
| 175 |
|
---|
| 176 | vec<N,T> operator * ( T d) const
|
---|
| 177 | { vec<N,T> rt(*this); return rt *= d; }
|
---|
| 178 |
|
---|
| 179 | friend bool operator == TEMPLATE_FUNCTION ( const vec<N,T> &v1, const vec<N,T> &v2 );
|
---|
| 180 | friend bool operator != TEMPLATE_FUNCTION ( const vec<N,T> &v1, const vec<N,T> &v2 );
|
---|
| 181 |
|
---|
| 182 |
|
---|
| 183 | //protected:
|
---|
| 184 | T v[N];
|
---|
| 185 | };
|
---|
| 186 |
|
---|
| 187 |
|
---|
| 188 |
|
---|
| 189 | // vector friend operators
|
---|
| 190 |
|
---|
| 191 | template <int N, class T> inline
|
---|
| 192 | vec<N,T> operator * ( const vec<N,T> & b, T d )
|
---|
| 193 | {
|
---|
| 194 | vec<N,T> rt(b);
|
---|
| 195 | return rt *= d;
|
---|
| 196 | }
|
---|
| 197 |
|
---|
| 198 | template <int N, class T> inline
|
---|
| 199 | vec<N,T> operator * ( T d, const vec<N,T> & b )
|
---|
| 200 | { return b*d; }
|
---|
| 201 |
|
---|
| 202 | template <int N, class T> inline
|
---|
| 203 | vec<N,T> operator * ( const vec<N,T> & b, const vec<N,T> & d )
|
---|
| 204 | {
|
---|
| 205 | vec<N,T> rt(b);
|
---|
| 206 | return rt *= d;
|
---|
| 207 | }
|
---|
| 208 |
|
---|
| 209 | template <int N, class T> inline
|
---|
| 210 | vec<N,T> operator / ( const vec<N,T> & b, T d )
|
---|
| 211 | { vec<N,T> rt(b); return rt /= d; }
|
---|
| 212 |
|
---|
| 213 | template <int N, class T> inline
|
---|
| 214 | vec<N,T> operator + ( const vec<N,T> & v1, const vec<N,T> & v2 )
|
---|
| 215 | { vec<N,T> rt(v1); return rt += v2; }
|
---|
| 216 |
|
---|
| 217 | template <int N, class T> inline
|
---|
| 218 | vec<N,T> operator - ( const vec<N,T> & v1, const vec<N,T> & v2 )
|
---|
| 219 | { vec<N,T> rt(v1); return rt -= v2; }
|
---|
| 220 |
|
---|
| 221 |
|
---|
| 222 | template <int N, class T> inline
|
---|
| 223 | bool operator == ( const vec<N,T> & v1, const vec<N,T> & v2 )
|
---|
| 224 | {
|
---|
| 225 | for(int i = 0; i < N; i++)
|
---|
| 226 | if(v1.v[i] != v2.v[i])
|
---|
| 227 | return false;
|
---|
| 228 | return true;
|
---|
| 229 | }
|
---|
| 230 |
|
---|
| 231 | template <int N, class T> inline
|
---|
| 232 | bool operator != ( const vec<N,T> & v1, const vec<N,T> & v2 )
|
---|
| 233 | { return !(v1 == v2); }
|
---|
| 234 |
|
---|
| 235 |
|
---|
| 236 | typedef vec<3,unsigned char> vec3ub;
|
---|
| 237 | typedef vec<4,unsigned char> vec4ub;
|
---|
| 238 |
|
---|
| 239 |
|
---|
| 240 |
|
---|
| 241 |
|
---|
| 242 |
|
---|
| 243 | namespace GLH_REAL_NAMESPACE
|
---|
| 244 | {
|
---|
| 245 | typedef GLH_REAL real;
|
---|
| 246 |
|
---|
| 247 | class line;
|
---|
| 248 | class plane;
|
---|
| 249 | class matrix4;
|
---|
| 250 | class quaternion;
|
---|
| 251 | typedef quaternion rotation;
|
---|
| 252 |
|
---|
| 253 | class vec2 : public vec<2,real>
|
---|
| 254 | {
|
---|
| 255 | public:
|
---|
| 256 | vec2(const real & t = real()) : vec<2,real>(t)
|
---|
| 257 | {}
|
---|
| 258 | vec2(const vec<2,real> & t) : vec<2,real>(t)
|
---|
| 259 | {}
|
---|
| 260 | vec2(const real * tp) : vec<2,real>(tp)
|
---|
| 261 | {}
|
---|
| 262 |
|
---|
| 263 | vec2(real x, real y )
|
---|
| 264 | { v[0] = x; v[1] = y; }
|
---|
| 265 |
|
---|
| 266 | void get_value(real & x, real & y) const
|
---|
| 267 | { x = v[0]; y = v[1]; }
|
---|
| 268 |
|
---|
| 269 | vec2 & set_value( const real & x, const real & y)
|
---|
| 270 | { v[0] = x; v[1] = y; return *this; }
|
---|
| 271 |
|
---|
| 272 | };
|
---|
| 273 |
|
---|
| 274 |
|
---|
| 275 | class vec3 : public vec<3,real>
|
---|
| 276 | {
|
---|
| 277 | public:
|
---|
| 278 | vec3(const real & t = real()) : vec<3,real>(t)
|
---|
| 279 | {}
|
---|
| 280 | vec3(const vec<3,real> & t) : vec<3,real>(t)
|
---|
| 281 | {}
|
---|
| 282 | vec3(const real * tp) : vec<3,real>(tp)
|
---|
| 283 | {}
|
---|
| 284 |
|
---|
| 285 | vec3(real x, real y, real z)
|
---|
| 286 | { v[0] = x; v[1] = y; v[2] = z; }
|
---|
| 287 |
|
---|
| 288 | void get_value(real & x, real & y, real & z) const
|
---|
| 289 | { x = v[0]; y = v[1]; z = v[2]; }
|
---|
| 290 |
|
---|
| 291 | vec3 cross( const vec3 &rhs ) const
|
---|
| 292 | {
|
---|
| 293 | vec3 rt;
|
---|
| 294 | rt.v[0] = v[1]*rhs.v[2]-v[2]*rhs.v[1];
|
---|
| 295 | rt.v[1] = v[2]*rhs.v[0]-v[0]*rhs.v[2];
|
---|
| 296 | rt.v[2] = v[0]*rhs.v[1]-v[1]*rhs.v[0];
|
---|
| 297 | return rt;
|
---|
| 298 | }
|
---|
| 299 |
|
---|
| 300 | vec3 & set_value( const real & x, const real & y, const real & z)
|
---|
| 301 | { v[0] = x; v[1] = y; v[2] = z; return *this; }
|
---|
| 302 |
|
---|
| 303 | };
|
---|
| 304 |
|
---|
| 305 |
|
---|
| 306 | class vec4 : public vec<4,real>
|
---|
| 307 | {
|
---|
| 308 | public:
|
---|
| 309 | vec4(const real & t = real()) : vec<4,real>(t)
|
---|
| 310 | {}
|
---|
| 311 | vec4(const vec<4,real> & t) : vec<4,real>(t)
|
---|
| 312 | {}
|
---|
| 313 |
|
---|
| 314 | vec4(const vec<3,real> & t, real fourth)
|
---|
| 315 |
|
---|
| 316 | { v[0] = t.v[0]; v[1] = t.v[1]; v[2] = t.v[2]; v[3] = fourth; }
|
---|
| 317 | vec4(const real * tp) : vec<4,real>(tp)
|
---|
| 318 | {}
|
---|
| 319 | vec4(real x, real y, real z, real w)
|
---|
| 320 | { v[0] = x; v[1] = y; v[2] = z; v[3] = w; }
|
---|
| 321 |
|
---|
| 322 | void get_value(real & x, real & y, real & z, real & w) const
|
---|
| 323 | { x = v[0]; y = v[1]; z = v[2]; w = v[3]; }
|
---|
| 324 |
|
---|
| 325 | vec4 & set_value( const real & x, const real & y, const real & z, const real & w)
|
---|
| 326 | { v[0] = x; v[1] = y; v[2] = z; v[3] = w; return *this; }
|
---|
| 327 | };
|
---|
| 328 |
|
---|
| 329 | inline
|
---|
| 330 | vec3 homogenize(const vec4 & v)
|
---|
| 331 | {
|
---|
| 332 | vec3 rt;
|
---|
| 333 | assert(v.v[3] != GLH_ZERO);
|
---|
| 334 | rt.v[0] = v.v[0]/v.v[3];
|
---|
| 335 | rt.v[1] = v.v[1]/v.v[3];
|
---|
| 336 | rt.v[2] = v.v[2]/v.v[3];
|
---|
| 337 | return rt;
|
---|
| 338 | }
|
---|
| 339 |
|
---|
| 340 |
|
---|
| 341 |
|
---|
| 342 | class line
|
---|
| 343 | {
|
---|
| 344 | public:
|
---|
| 345 |
|
---|
| 346 | line()
|
---|
| 347 | { set_value(vec3(0,0,0),vec3(0,0,1)); }
|
---|
| 348 |
|
---|
| 349 | line( const vec3 & p0, const vec3 &p1)
|
---|
| 350 | { set_value(p0,p1); }
|
---|
| 351 |
|
---|
| 352 | void set_value( const vec3 &p0, const vec3 &p1)
|
---|
| 353 | {
|
---|
| 354 | position = p0;
|
---|
| 355 | direction = p1-p0;
|
---|
| 356 | direction.normalize();
|
---|
| 357 | }
|
---|
| 358 |
|
---|
| 359 | bool get_closest_points(const line &line2,
|
---|
| 360 | vec3 &pointOnThis,
|
---|
| 361 | vec3 &pointOnThat)
|
---|
| 362 | {
|
---|
| 363 |
|
---|
| 364 | // quick check to see if parallel -- if so, quit.
|
---|
| 365 | if(fabs(direction.dot(line2.direction)) == 1.0)
|
---|
| 366 | return 0;
|
---|
| 367 | line l2 = line2;
|
---|
| 368 |
|
---|
| 369 | // Algorithm: Brian Jean
|
---|
| 370 | //
|
---|
| 371 | register real u;
|
---|
| 372 | register real v;
|
---|
| 373 | vec3 Vr = direction;
|
---|
| 374 | vec3 Vs = l2.direction;
|
---|
| 375 | register real Vr_Dot_Vs = Vr.dot(Vs);
|
---|
| 376 | register real detA = real(1.0 - (Vr_Dot_Vs * Vr_Dot_Vs));
|
---|
| 377 | vec3 C = l2.position - position;
|
---|
| 378 | register real C_Dot_Vr = C.dot(Vr);
|
---|
| 379 | register real C_Dot_Vs = C.dot(Vs);
|
---|
| 380 |
|
---|
| 381 | u = (C_Dot_Vr - Vr_Dot_Vs * C_Dot_Vs)/detA;
|
---|
| 382 | v = (C_Dot_Vr * Vr_Dot_Vs - C_Dot_Vs)/detA;
|
---|
| 383 |
|
---|
| 384 | pointOnThis = position;
|
---|
| 385 | pointOnThis += direction * u;
|
---|
| 386 | pointOnThat = l2.position;
|
---|
| 387 | pointOnThat += l2.direction * v;
|
---|
| 388 |
|
---|
| 389 | return 1;
|
---|
| 390 | }
|
---|
| 391 |
|
---|
| 392 | vec3 get_closest_point(const vec3 &point)
|
---|
| 393 | {
|
---|
| 394 | vec3 np = point - position;
|
---|
| 395 | vec3 rp = direction*direction.dot(np)+position;
|
---|
| 396 | return rp;
|
---|
| 397 | }
|
---|
| 398 |
|
---|
| 399 | const vec3 & get_position() const {return position;}
|
---|
| 400 |
|
---|
| 401 | const vec3 & get_direction() const {return direction;}
|
---|
| 402 |
|
---|
| 403 | //protected:
|
---|
| 404 | vec3 position;
|
---|
| 405 | vec3 direction;
|
---|
| 406 | };
|
---|
| 407 |
|
---|
| 408 |
|
---|
| 409 |
|
---|
| 410 |
|
---|
| 411 |
|
---|
| 412 |
|
---|
| 413 |
|
---|
| 414 |
|
---|
| 415 |
|
---|
| 416 |
|
---|
| 417 |
|
---|
| 418 |
|
---|
| 419 |
|
---|
| 420 |
|
---|
| 421 |
|
---|
| 422 |
|
---|
| 423 |
|
---|
| 424 |
|
---|
| 425 |
|
---|
| 426 |
|
---|
| 427 |
|
---|
| 428 |
|
---|
| 429 |
|
---|
| 430 |
|
---|
| 431 |
|
---|
| 432 |
|
---|
| 433 |
|
---|
| 434 |
|
---|
| 435 |
|
---|
| 436 | // matrix
|
---|
| 437 |
|
---|
| 438 |
|
---|
| 439 | class matrix4
|
---|
| 440 | {
|
---|
| 441 |
|
---|
| 442 | public:
|
---|
| 443 |
|
---|
| 444 | matrix4() { make_identity(); }
|
---|
| 445 |
|
---|
| 446 | matrix4( real r )
|
---|
| 447 | { set_value(r); }
|
---|
| 448 |
|
---|
| 449 | matrix4( real * m )
|
---|
| 450 | { set_value(m); }
|
---|
| 451 |
|
---|
| 452 | matrix4( real a00, real a01, real a02, real a03,
|
---|
| 453 | real a10, real a11, real a12, real a13,
|
---|
| 454 | real a20, real a21, real a22, real a23,
|
---|
| 455 | real a30, real a31, real a32, real a33 )
|
---|
| 456 | {
|
---|
| 457 | element(0,0) = a00;
|
---|
| 458 | element(0,1) = a01;
|
---|
| 459 | element(0,2) = a02;
|
---|
| 460 | element(0,3) = a03;
|
---|
| 461 |
|
---|
| 462 | element(1,0) = a10;
|
---|
| 463 | element(1,1) = a11;
|
---|
| 464 | element(1,2) = a12;
|
---|
| 465 | element(1,3) = a13;
|
---|
| 466 |
|
---|
| 467 | element(2,0) = a20;
|
---|
| 468 | element(2,1) = a21;
|
---|
| 469 | element(2,2) = a22;
|
---|
| 470 | element(2,3) = a23;
|
---|
| 471 |
|
---|
| 472 | element(3,0) = a30;
|
---|
| 473 | element(3,1) = a31;
|
---|
| 474 | element(3,2) = a32;
|
---|
| 475 | element(3,3) = a33;
|
---|
| 476 | }
|
---|
| 477 |
|
---|
| 478 |
|
---|
| 479 | void get_value( real * mp ) const
|
---|
| 480 | {
|
---|
| 481 | int c = 0;
|
---|
| 482 | for(int j=0; j < 4; j++)
|
---|
| 483 | for(int i=0; i < 4; i++)
|
---|
| 484 | mp[c++] = element(i,j);
|
---|
| 485 | }
|
---|
| 486 |
|
---|
| 487 |
|
---|
| 488 | const real * get_value() const
|
---|
| 489 | { return m; }
|
---|
| 490 |
|
---|
| 491 | void set_value( real * mp)
|
---|
| 492 | {
|
---|
| 493 | int c = 0;
|
---|
| 494 | for(int j=0; j < 4; j++)
|
---|
| 495 | for(int i=0; i < 4; i++)
|
---|
| 496 | element(i,j) = mp[c++];
|
---|
| 497 | }
|
---|
| 498 |
|
---|
| 499 | void set_value( real r )
|
---|
| 500 | {
|
---|
| 501 | for(int i=0; i < 4; i++)
|
---|
| 502 | for(int j=0; j < 4; j++)
|
---|
| 503 | element(i,j) = r;
|
---|
| 504 | }
|
---|
| 505 |
|
---|
| 506 | void make_identity()
|
---|
| 507 | {
|
---|
| 508 | element(0,0) = 1.0;
|
---|
| 509 | element(0,1) = 0.0;
|
---|
| 510 | element(0,2) = 0.0;
|
---|
| 511 | element(0,3) = 0.0;
|
---|
| 512 |
|
---|
| 513 | element(1,0) = 0.0;
|
---|
| 514 | element(1,1) = 1.0;
|
---|
| 515 | element(1,2) = 0.0;
|
---|
| 516 | element(1,3) = 0.0;
|
---|
| 517 |
|
---|
| 518 | element(2,0) = 0.0;
|
---|
| 519 | element(2,1) = 0.0;
|
---|
| 520 | element(2,2) = 1.0;
|
---|
| 521 | element(2,3) = 0.0;
|
---|
| 522 |
|
---|
| 523 | element(3,0) = 0.0;
|
---|
| 524 | element(3,1) = 0.0;
|
---|
| 525 | element(3,2) = 0.0;
|
---|
| 526 | element(3,3) = 1.0;
|
---|
| 527 | }
|
---|
| 528 |
|
---|
| 529 |
|
---|
| 530 | static matrix4 identity()
|
---|
| 531 | {
|
---|
| 532 | static matrix4 mident (
|
---|
| 533 | 1.0, 0.0, 0.0, 0.0,
|
---|
| 534 | 0.0, 1.0, 0.0, 0.0,
|
---|
| 535 | 0.0, 0.0, 1.0, 0.0,
|
---|
| 536 | 0.0, 0.0, 0.0, 1.0 );
|
---|
| 537 | return mident;
|
---|
| 538 | }
|
---|
| 539 |
|
---|
| 540 |
|
---|
| 541 | void set_scale( real s )
|
---|
| 542 | {
|
---|
| 543 | element(0,0) = s;
|
---|
| 544 | element(1,1) = s;
|
---|
| 545 | element(2,2) = s;
|
---|
| 546 | }
|
---|
| 547 |
|
---|
| 548 | void set_scale( const vec3 & s )
|
---|
| 549 | {
|
---|
| 550 | element(0,0) = s.v[0];
|
---|
| 551 | element(1,1) = s.v[1];
|
---|
| 552 | element(2,2) = s.v[2];
|
---|
| 553 | }
|
---|
| 554 |
|
---|
| 555 |
|
---|
| 556 | void set_translate( const vec3 & t )
|
---|
| 557 | {
|
---|
| 558 | element(0,3) = t.v[0];
|
---|
| 559 | element(1,3) = t.v[1];
|
---|
| 560 | element(2,3) = t.v[2];
|
---|
| 561 | }
|
---|
| 562 |
|
---|
| 563 | void set_row(int r, const vec4 & t)
|
---|
| 564 | {
|
---|
| 565 | element(r,0) = t.v[0];
|
---|
| 566 | element(r,1) = t.v[1];
|
---|
| 567 | element(r,2) = t.v[2];
|
---|
| 568 | element(r,3) = t.v[3];
|
---|
| 569 | }
|
---|
| 570 |
|
---|
| 571 | void set_column(int c, const vec4 & t)
|
---|
| 572 | {
|
---|
| 573 | element(0,c) = t.v[0];
|
---|
| 574 | element(1,c) = t.v[1];
|
---|
| 575 | element(2,c) = t.v[2];
|
---|
| 576 | element(3,c) = t.v[3];
|
---|
| 577 | }
|
---|
| 578 |
|
---|
| 579 |
|
---|
| 580 | void get_row(int r, vec4 & t) const
|
---|
| 581 | {
|
---|
| 582 | t.v[0] = element(r,0);
|
---|
| 583 | t.v[1] = element(r,1);
|
---|
| 584 | t.v[2] = element(r,2);
|
---|
| 585 | t.v[3] = element(r,3);
|
---|
| 586 | }
|
---|
| 587 |
|
---|
| 588 | vec4 get_row(int r) const
|
---|
| 589 | {
|
---|
| 590 | vec4 v; get_row(r, v);
|
---|
| 591 | return v;
|
---|
| 592 | }
|
---|
| 593 |
|
---|
| 594 | void get_column(int c, vec4 & t) const
|
---|
| 595 | {
|
---|
| 596 | t.v[0] = element(0,c);
|
---|
| 597 | t.v[1] = element(1,c);
|
---|
| 598 | t.v[2] = element(2,c);
|
---|
| 599 | t.v[3] = element(3,c);
|
---|
| 600 | }
|
---|
| 601 |
|
---|
| 602 | vec4 get_column(int c) const
|
---|
| 603 | {
|
---|
| 604 | vec4 v; get_column(c, v);
|
---|
| 605 | return v;
|
---|
| 606 | }
|
---|
| 607 |
|
---|
| 608 | matrix4 inverse() const
|
---|
| 609 | {
|
---|
| 610 | matrix4 minv;
|
---|
| 611 |
|
---|
| 612 | real r1[8], r2[8], r3[8], r4[8];
|
---|
| 613 | real *s[4], *tmprow;
|
---|
| 614 |
|
---|
| 615 | s[0] = &r1[0];
|
---|
| 616 | s[1] = &r2[0];
|
---|
| 617 | s[2] = &r3[0];
|
---|
| 618 | s[3] = &r4[0];
|
---|
| 619 |
|
---|
| 620 | register int i,j,p,jj;
|
---|
| 621 | for(i=0;i<4;i++)
|
---|
| 622 | {
|
---|
| 623 | for(j=0;j<4;j++)
|
---|
| 624 | {
|
---|
| 625 | s[i][j] = element(i,j);
|
---|
| 626 | if(i==j) s[i][j+4] = 1.0;
|
---|
| 627 | else s[i][j+4] = 0.0;
|
---|
| 628 | }
|
---|
| 629 | }
|
---|
| 630 | real scp[4];
|
---|
| 631 | for(i=0;i<4;i++)
|
---|
| 632 | {
|
---|
| 633 | scp[i] = real(fabs(s[i][0]));
|
---|
| 634 | for(j=1;j<4;j++)
|
---|
| 635 | if(real(fabs(s[i][j])) > scp[i]) scp[i] = real(fabs(s[i][j]));
|
---|
| 636 | if(scp[i] == 0.0) return minv; // singular matrix!
|
---|
| 637 | }
|
---|
| 638 |
|
---|
| 639 | int pivot_to;
|
---|
| 640 | real scp_max;
|
---|
| 641 | for(i=0;i<4;i++)
|
---|
| 642 | {
|
---|
| 643 | // select pivot row
|
---|
| 644 | pivot_to = i;
|
---|
| 645 | scp_max = real(fabs(s[i][i]/scp[i]));
|
---|
| 646 | // find out which row should be on top
|
---|
| 647 | for(p=i+1;p<4;p++)
|
---|
| 648 | if(real(fabs(s[p][i]/scp[p])) > scp_max)
|
---|
| 649 | { scp_max = real(fabs(s[p][i]/scp[p])); pivot_to = p; }
|
---|
| 650 | // Pivot if necessary
|
---|
| 651 | if(pivot_to != i)
|
---|
| 652 | {
|
---|
| 653 | tmprow = s[i];
|
---|
| 654 | s[i] = s[pivot_to];
|
---|
| 655 | s[pivot_to] = tmprow;
|
---|
| 656 | real tmpscp;
|
---|
| 657 | tmpscp = scp[i];
|
---|
| 658 | scp[i] = scp[pivot_to];
|
---|
| 659 | scp[pivot_to] = tmpscp;
|
---|
| 660 | }
|
---|
| 661 |
|
---|
| 662 | real mji;
|
---|
| 663 | // perform gaussian elimination
|
---|
| 664 | for(j=i+1;j<4;j++)
|
---|
| 665 | {
|
---|
| 666 | mji = s[j][i]/s[i][i];
|
---|
| 667 | s[j][i] = 0.0;
|
---|
| 668 | for(jj=i+1;jj<8;jj++)
|
---|
| 669 | s[j][jj] -= mji*s[i][jj];
|
---|
| 670 | }
|
---|
| 671 | }
|
---|
| 672 | if(s[3][3] == 0.0) return minv; // singular matrix!
|
---|
| 673 |
|
---|
| 674 | //
|
---|
| 675 | // Now we have an upper triangular matrix.
|
---|
| 676 | //
|
---|
| 677 | // x x x x | y y y y
|
---|
| 678 | // 0 x x x | y y y y
|
---|
| 679 | // 0 0 x x | y y y y
|
---|
| 680 | // 0 0 0 x | y y y y
|
---|
| 681 | //
|
---|
| 682 | // we'll back substitute to get the inverse
|
---|
| 683 | //
|
---|
| 684 | // 1 0 0 0 | z z z z
|
---|
| 685 | // 0 1 0 0 | z z z z
|
---|
| 686 | // 0 0 1 0 | z z z z
|
---|
| 687 | // 0 0 0 1 | z z z z
|
---|
| 688 | //
|
---|
| 689 |
|
---|
| 690 | real mij;
|
---|
| 691 | for(i=3;i>0;i--)
|
---|
| 692 | {
|
---|
| 693 | for(j=i-1;j > -1; j--)
|
---|
| 694 | {
|
---|
| 695 | mij = s[j][i]/s[i][i];
|
---|
| 696 | for(jj=j+1;jj<8;jj++)
|
---|
| 697 | s[j][jj] -= mij*s[i][jj];
|
---|
| 698 | }
|
---|
| 699 | }
|
---|
| 700 |
|
---|
| 701 | for(i=0;i<4;i++)
|
---|
| 702 | for(j=0;j<4;j++)
|
---|
| 703 | minv(i,j) = s[i][j+4] / s[i][i];
|
---|
| 704 |
|
---|
| 705 | return minv;
|
---|
| 706 | }
|
---|
| 707 |
|
---|
| 708 |
|
---|
| 709 | matrix4 transpose() const
|
---|
| 710 | {
|
---|
| 711 | matrix4 mtrans;
|
---|
| 712 |
|
---|
| 713 | for(int i=0;i<4;i++)
|
---|
| 714 | for(int j=0;j<4;j++)
|
---|
| 715 | mtrans(i,j) = element(j,i);
|
---|
| 716 | return mtrans;
|
---|
| 717 | }
|
---|
| 718 |
|
---|
| 719 | matrix4 & mult_right( const matrix4 & b )
|
---|
| 720 | {
|
---|
| 721 | matrix4 mt(*this);
|
---|
| 722 | set_value(real(0));
|
---|
| 723 |
|
---|
| 724 | for(int i=0; i < 4; i++)
|
---|
| 725 | for(int j=0; j < 4; j++)
|
---|
| 726 | for(int c=0; c < 4; c++)
|
---|
| 727 | element(i,j) += mt(i,c) * b(c,j);
|
---|
| 728 | return *this;
|
---|
| 729 | }
|
---|
| 730 |
|
---|
| 731 | matrix4 & mult_left( const matrix4 & b )
|
---|
| 732 | {
|
---|
| 733 | matrix4 mt(*this);
|
---|
| 734 | set_value(real(0));
|
---|
| 735 |
|
---|
| 736 | for(int i=0; i < 4; i++)
|
---|
| 737 | for(int j=0; j < 4; j++)
|
---|
| 738 | for(int c=0; c < 4; c++)
|
---|
| 739 | element(i,j) += b(i,c) * mt(c,j);
|
---|
| 740 | return *this;
|
---|
| 741 | }
|
---|
| 742 |
|
---|
| 743 | // dst = M * src
|
---|
| 744 | void mult_matrix_vec( const vec3 &src, vec3 &dst ) const
|
---|
| 745 | {
|
---|
| 746 | real w = (
|
---|
| 747 | src.v[0] * element(3,0) +
|
---|
| 748 | src.v[1] * element(3,1) +
|
---|
| 749 | src.v[2] * element(3,2) +
|
---|
| 750 | element(3,3) );
|
---|
| 751 |
|
---|
| 752 | assert(w != GLH_ZERO);
|
---|
| 753 |
|
---|
| 754 | dst.v[0] = (
|
---|
| 755 | src.v[0] * element(0,0) +
|
---|
| 756 | src.v[1] * element(0,1) +
|
---|
| 757 | src.v[2] * element(0,2) +
|
---|
| 758 | element(0,3) ) / w;
|
---|
| 759 | dst.v[1] = (
|
---|
| 760 | src.v[0] * element(1,0) +
|
---|
| 761 | src.v[1] * element(1,1) +
|
---|
| 762 | src.v[2] * element(1,2) +
|
---|
| 763 | element(1,3) ) / w;
|
---|
| 764 | dst.v[2] = (
|
---|
| 765 | src.v[0] * element(2,0) +
|
---|
| 766 | src.v[1] * element(2,1) +
|
---|
| 767 | src.v[2] * element(2,2) +
|
---|
| 768 | element(2,3) ) / w;
|
---|
| 769 | }
|
---|
| 770 |
|
---|
| 771 | void mult_matrix_vec( vec3 & src_and_dst) const
|
---|
| 772 | { mult_matrix_vec(vec3(src_and_dst), src_and_dst); }
|
---|
| 773 |
|
---|
| 774 |
|
---|
| 775 | // dst = src * M
|
---|
| 776 | void mult_vec_matrix( const vec3 &src, vec3 &dst ) const
|
---|
| 777 | {
|
---|
| 778 | real w = (
|
---|
| 779 | src.v[0] * element(0,3) +
|
---|
| 780 | src.v[1] * element(1,3) +
|
---|
| 781 | src.v[2] * element(2,3) +
|
---|
| 782 | element(3,3) );
|
---|
| 783 |
|
---|
| 784 | assert(w != GLH_ZERO);
|
---|
| 785 |
|
---|
| 786 | dst.v[0] = (
|
---|
| 787 | src.v[0] * element(0,0) +
|
---|
| 788 | src.v[1] * element(1,0) +
|
---|
| 789 | src.v[2] * element(2,0) +
|
---|
| 790 | element(3,0) ) / w;
|
---|
| 791 | dst.v[1] = (
|
---|
| 792 | src.v[0] * element(0,1) +
|
---|
| 793 | src.v[1] * element(1,1) +
|
---|
| 794 | src.v[2] * element(2,1) +
|
---|
| 795 | element(3,1) ) / w;
|
---|
| 796 | dst.v[2] = (
|
---|
| 797 | src.v[0] * element(0,2) +
|
---|
| 798 | src.v[1] * element(1,2) +
|
---|
| 799 | src.v[2] * element(2,2) +
|
---|
| 800 | element(3,2) ) / w;
|
---|
| 801 | }
|
---|
| 802 |
|
---|
| 803 |
|
---|
| 804 | void mult_vec_matrix( vec3 & src_and_dst) const
|
---|
| 805 | { mult_vec_matrix(vec3(src_and_dst), src_and_dst); }
|
---|
| 806 |
|
---|
| 807 | // dst = M * src
|
---|
| 808 | void mult_matrix_vec( const vec4 &src, vec4 &dst ) const
|
---|
| 809 | {
|
---|
| 810 | dst.v[0] = (
|
---|
| 811 | src.v[0] * element(0,0) +
|
---|
| 812 | src.v[1] * element(0,1) +
|
---|
| 813 | src.v[2] * element(0,2) +
|
---|
| 814 | src.v[3] * element(0,3));
|
---|
| 815 | dst.v[1] = (
|
---|
| 816 | src.v[0] * element(1,0) +
|
---|
| 817 | src.v[1] * element(1,1) +
|
---|
| 818 | src.v[2] * element(1,2) +
|
---|
| 819 | src.v[3] * element(1,3));
|
---|
| 820 | dst.v[2] = (
|
---|
| 821 | src.v[0] * element(2,0) +
|
---|
| 822 | src.v[1] * element(2,1) +
|
---|
| 823 | src.v[2] * element(2,2) +
|
---|
| 824 | src.v[3] * element(2,3));
|
---|
| 825 | dst.v[3] = (
|
---|
| 826 | src.v[0] * element(3,0) +
|
---|
| 827 | src.v[1] * element(3,1) +
|
---|
| 828 | src.v[2] * element(3,2) +
|
---|
| 829 | src.v[3] * element(3,3));
|
---|
| 830 | }
|
---|
| 831 |
|
---|
| 832 | void mult_matrix_vec( vec4 & src_and_dst) const
|
---|
| 833 | { mult_matrix_vec(vec4(src_and_dst), src_and_dst); }
|
---|
| 834 |
|
---|
| 835 |
|
---|
| 836 | // dst = src * M
|
---|
| 837 | void mult_vec_matrix( const vec4 &src, vec4 &dst ) const
|
---|
| 838 | {
|
---|
| 839 | dst.v[0] = (
|
---|
| 840 | src.v[0] * element(0,0) +
|
---|
| 841 | src.v[1] * element(1,0) +
|
---|
| 842 | src.v[2] * element(2,0) +
|
---|
| 843 | src.v[3] * element(3,0));
|
---|
| 844 | dst.v[1] = (
|
---|
| 845 | src.v[0] * element(0,1) +
|
---|
| 846 | src.v[1] * element(1,1) +
|
---|
| 847 | src.v[2] * element(2,1) +
|
---|
| 848 | src.v[3] * element(3,1));
|
---|
| 849 | dst.v[2] = (
|
---|
| 850 | src.v[0] * element(0,2) +
|
---|
| 851 | src.v[1] * element(1,2) +
|
---|
| 852 | src.v[2] * element(2,2) +
|
---|
| 853 | src.v[3] * element(3,2));
|
---|
| 854 | dst.v[3] = (
|
---|
| 855 | src.v[0] * element(0,3) +
|
---|
| 856 | src.v[1] * element(1,3) +
|
---|
| 857 | src.v[2] * element(2,3) +
|
---|
| 858 | src.v[3] * element(3,3));
|
---|
| 859 | }
|
---|
| 860 |
|
---|
| 861 |
|
---|
| 862 | void mult_vec_matrix( vec4 & src_and_dst) const
|
---|
| 863 | { mult_vec_matrix(vec4(src_and_dst), src_and_dst); }
|
---|
| 864 |
|
---|
| 865 |
|
---|
| 866 | // dst = M * src
|
---|
| 867 | void mult_matrix_dir( const vec3 &src, vec3 &dst ) const
|
---|
| 868 | {
|
---|
| 869 | dst.v[0] = (
|
---|
| 870 | src.v[0] * element(0,0) +
|
---|
| 871 | src.v[1] * element(0,1) +
|
---|
| 872 | src.v[2] * element(0,2) ) ;
|
---|
| 873 | dst.v[1] = (
|
---|
| 874 | src.v[0] * element(1,0) +
|
---|
| 875 | src.v[1] * element(1,1) +
|
---|
| 876 | src.v[2] * element(1,2) ) ;
|
---|
| 877 | dst.v[2] = (
|
---|
| 878 | src.v[0] * element(2,0) +
|
---|
| 879 | src.v[1] * element(2,1) +
|
---|
| 880 | src.v[2] * element(2,2) ) ;
|
---|
| 881 | }
|
---|
| 882 |
|
---|
| 883 |
|
---|
| 884 | void mult_matrix_dir( vec3 & src_and_dst) const
|
---|
| 885 | { mult_matrix_dir(vec3(src_and_dst), src_and_dst); }
|
---|
| 886 |
|
---|
| 887 |
|
---|
| 888 | // dst = src * M
|
---|
| 889 | void mult_dir_matrix( const vec3 &src, vec3 &dst ) const
|
---|
| 890 | {
|
---|
| 891 | dst.v[0] = (
|
---|
| 892 | src.v[0] * element(0,0) +
|
---|
| 893 | src.v[1] * element(1,0) +
|
---|
| 894 | src.v[2] * element(2,0) ) ;
|
---|
| 895 | dst.v[1] = (
|
---|
| 896 | src.v[0] * element(0,1) +
|
---|
| 897 | src.v[1] * element(1,1) +
|
---|
| 898 | src.v[2] * element(2,1) ) ;
|
---|
| 899 | dst.v[2] = (
|
---|
| 900 | src.v[0] * element(0,2) +
|
---|
| 901 | src.v[1] * element(1,2) +
|
---|
| 902 | src.v[2] * element(2,2) ) ;
|
---|
| 903 | }
|
---|
| 904 |
|
---|
| 905 |
|
---|
| 906 | void mult_dir_matrix( vec3 & src_and_dst) const
|
---|
| 907 | { mult_dir_matrix(vec3(src_and_dst), src_and_dst); }
|
---|
| 908 |
|
---|
| 909 |
|
---|
| 910 | real & operator () (int row, int col)
|
---|
| 911 | { return element(row,col); }
|
---|
| 912 |
|
---|
| 913 | const real & operator () (int row, int col) const
|
---|
| 914 | { return element(row,col); }
|
---|
| 915 |
|
---|
| 916 | real & element (int row, int col)
|
---|
| 917 | { return m[row | (col<<2)]; }
|
---|
| 918 |
|
---|
| 919 | const real & element (int row, int col) const
|
---|
| 920 | { return m[row | (col<<2)]; }
|
---|
| 921 |
|
---|
| 922 | matrix4 & operator *= ( const matrix4 & mat )
|
---|
| 923 | {
|
---|
| 924 | mult_right( mat );
|
---|
| 925 | return *this;
|
---|
| 926 | }
|
---|
| 927 |
|
---|
| 928 | matrix4 & operator *= ( const real & r )
|
---|
| 929 | {
|
---|
| 930 | for (int i = 0; i < 4; ++i)
|
---|
| 931 | {
|
---|
| 932 | element(0,i) *= r;
|
---|
| 933 | element(1,i) *= r;
|
---|
| 934 | element(2,i) *= r;
|
---|
| 935 | element(3,i) *= r;
|
---|
| 936 | }
|
---|
| 937 | return *this;
|
---|
| 938 | }
|
---|
| 939 |
|
---|
| 940 | matrix4 & operator += ( const matrix4 & mat )
|
---|
| 941 | {
|
---|
| 942 | for (int i = 0; i < 4; ++i)
|
---|
| 943 | {
|
---|
| 944 | element(0,i) += mat.element(0,i);
|
---|
| 945 | element(1,i) += mat.element(1,i);
|
---|
| 946 | element(2,i) += mat.element(2,i);
|
---|
| 947 | element(3,i) += mat.element(3,i);
|
---|
| 948 | }
|
---|
| 949 | return *this;
|
---|
| 950 | }
|
---|
| 951 |
|
---|
| 952 | friend matrix4 operator * ( const matrix4 & m1, const matrix4 & m2 );
|
---|
| 953 | friend bool operator == ( const matrix4 & m1, const matrix4 & m2 );
|
---|
| 954 | friend bool operator != ( const matrix4 & m1, const matrix4 & m2 );
|
---|
| 955 |
|
---|
| 956 | //protected:
|
---|
| 957 | real m[16];
|
---|
| 958 | };
|
---|
| 959 |
|
---|
| 960 | inline
|
---|
| 961 | matrix4 operator * ( const matrix4 & m1, const matrix4 & m2 )
|
---|
| 962 | {
|
---|
| 963 | matrix4 product;
|
---|
| 964 |
|
---|
| 965 | product = m1;
|
---|
| 966 | product.mult_right(m2);
|
---|
| 967 |
|
---|
| 968 | return product;
|
---|
| 969 | }
|
---|
| 970 |
|
---|
| 971 | inline
|
---|
| 972 | bool operator ==( const matrix4 &m1, const matrix4 &m2 )
|
---|
| 973 | {
|
---|
| 974 | return (
|
---|
| 975 | m1(0,0) == m2(0,0) &&
|
---|
| 976 | m1(0,1) == m2(0,1) &&
|
---|
| 977 | m1(0,2) == m2(0,2) &&
|
---|
| 978 | m1(0,3) == m2(0,3) &&
|
---|
| 979 | m1(1,0) == m2(1,0) &&
|
---|
| 980 | m1(1,1) == m2(1,1) &&
|
---|
| 981 | m1(1,2) == m2(1,2) &&
|
---|
| 982 | m1(1,3) == m2(1,3) &&
|
---|
| 983 | m1(2,0) == m2(2,0) &&
|
---|
| 984 | m1(2,1) == m2(2,1) &&
|
---|
| 985 | m1(2,2) == m2(2,2) &&
|
---|
| 986 | m1(2,3) == m2(2,3) &&
|
---|
| 987 | m1(3,0) == m2(3,0) &&
|
---|
| 988 | m1(3,1) == m2(3,1) &&
|
---|
| 989 | m1(3,2) == m2(3,2) &&
|
---|
| 990 | m1(3,3) == m2(3,3) );
|
---|
| 991 | }
|
---|
| 992 |
|
---|
| 993 | inline
|
---|
| 994 | bool operator != ( const matrix4 & m1, const matrix4 & m2 )
|
---|
| 995 | { return !( m1 == m2 ); }
|
---|
| 996 |
|
---|
| 997 |
|
---|
| 998 |
|
---|
| 999 |
|
---|
| 1000 |
|
---|
| 1001 |
|
---|
| 1002 |
|
---|
| 1003 |
|
---|
| 1004 |
|
---|
| 1005 |
|
---|
| 1006 |
|
---|
| 1007 |
|
---|
| 1008 |
|
---|
| 1009 | class quaternion
|
---|
| 1010 | {
|
---|
| 1011 | public:
|
---|
| 1012 |
|
---|
| 1013 | quaternion()
|
---|
| 1014 | {
|
---|
| 1015 | *this = identity();
|
---|
| 1016 | }
|
---|
| 1017 |
|
---|
| 1018 | quaternion( const real v[4] )
|
---|
| 1019 | {
|
---|
| 1020 | set_value( v );
|
---|
| 1021 | }
|
---|
| 1022 |
|
---|
| 1023 |
|
---|
| 1024 | quaternion( real q0, real q1, real q2, real q3 )
|
---|
| 1025 | {
|
---|
| 1026 | set_value( q0, q1, q2, q3 );
|
---|
| 1027 | }
|
---|
| 1028 |
|
---|
| 1029 |
|
---|
| 1030 | quaternion( const matrix4 & m )
|
---|
| 1031 | {
|
---|
| 1032 | set_value( m );
|
---|
| 1033 | }
|
---|
| 1034 |
|
---|
| 1035 |
|
---|
| 1036 | quaternion( const vec3 &axis, real radians )
|
---|
| 1037 | {
|
---|
| 1038 | set_value( axis, radians );
|
---|
| 1039 | }
|
---|
| 1040 |
|
---|
| 1041 |
|
---|
| 1042 | quaternion( const vec3 &rotateFrom, const vec3 &rotateTo )
|
---|
| 1043 | {
|
---|
| 1044 | set_value( rotateFrom, rotateTo );
|
---|
| 1045 | }
|
---|
| 1046 |
|
---|
| 1047 | quaternion( const vec3 & from_look, const vec3 & from_up,
|
---|
| 1048 | const vec3 & to_look, const vec3& to_up)
|
---|
| 1049 | {
|
---|
| 1050 | set_value(from_look, from_up, to_look, to_up);
|
---|
| 1051 | }
|
---|
| 1052 |
|
---|
| 1053 | const real * get_value() const
|
---|
| 1054 | {
|
---|
| 1055 | return &q[0];
|
---|
| 1056 | }
|
---|
| 1057 |
|
---|
| 1058 | void get_value( real &q0, real &q1, real &q2, real &q3 ) const
|
---|
| 1059 | {
|
---|
| 1060 | q0 = q[0];
|
---|
| 1061 | q1 = q[1];
|
---|
| 1062 | q2 = q[2];
|
---|
| 1063 | q3 = q[3];
|
---|
| 1064 | }
|
---|
| 1065 |
|
---|
| 1066 | quaternion & set_value( real q0, real q1, real q2, real q3 )
|
---|
| 1067 | {
|
---|
| 1068 | q[0] = q0;
|
---|
| 1069 | q[1] = q1;
|
---|
| 1070 | q[2] = q2;
|
---|
| 1071 | q[3] = q3;
|
---|
| 1072 | counter = 0;
|
---|
| 1073 | return *this;
|
---|
| 1074 | }
|
---|
| 1075 |
|
---|
| 1076 | void get_value( vec3 &axis, real &radians ) const
|
---|
| 1077 | {
|
---|
| 1078 | radians = real(acos( q[3] ) * GLH_TWO);
|
---|
| 1079 | if ( radians == GLH_ZERO )
|
---|
| 1080 | axis = vec3( 0.0, 0.0, 1.0 );
|
---|
| 1081 | else
|
---|
| 1082 | {
|
---|
| 1083 | axis.v[0] = q[0];
|
---|
| 1084 | axis.v[1] = q[1];
|
---|
| 1085 | axis.v[2] = q[2];
|
---|
| 1086 | axis.normalize();
|
---|
| 1087 | }
|
---|
| 1088 | }
|
---|
| 1089 |
|
---|
| 1090 | void get_value( matrix4 & m ) const
|
---|
| 1091 | {
|
---|
| 1092 | real s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
|
---|
| 1093 |
|
---|
| 1094 | real norm = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
|
---|
| 1095 |
|
---|
| 1096 | s = (equivalent(norm,GLH_ZERO)) ? GLH_ZERO : ( GLH_TWO / norm );
|
---|
| 1097 |
|
---|
| 1098 | xs = q[0] * s;
|
---|
| 1099 | ys = q[1] * s;
|
---|
| 1100 | zs = q[2] * s;
|
---|
| 1101 |
|
---|
| 1102 | wx = q[3] * xs;
|
---|
| 1103 | wy = q[3] * ys;
|
---|
| 1104 | wz = q[3] * zs;
|
---|
| 1105 |
|
---|
| 1106 | xx = q[0] * xs;
|
---|
| 1107 | xy = q[0] * ys;
|
---|
| 1108 | xz = q[0] * zs;
|
---|
| 1109 |
|
---|
| 1110 | yy = q[1] * ys;
|
---|
| 1111 | yz = q[1] * zs;
|
---|
| 1112 | zz = q[2] * zs;
|
---|
| 1113 |
|
---|
| 1114 | m(0,0) = real( GLH_ONE - ( yy + zz ));
|
---|
| 1115 | m(1,0) = real ( xy + wz );
|
---|
| 1116 | m(2,0) = real ( xz - wy );
|
---|
| 1117 |
|
---|
| 1118 | m(0,1) = real ( xy - wz );
|
---|
| 1119 | m(1,1) = real ( GLH_ONE - ( xx + zz ));
|
---|
| 1120 | m(2,1) = real ( yz + wx );
|
---|
| 1121 |
|
---|
| 1122 | m(0,2) = real ( xz + wy );
|
---|
| 1123 | m(1,2) = real ( yz - wx );
|
---|
| 1124 | m(2,2) = real ( GLH_ONE - ( xx + yy ));
|
---|
| 1125 |
|
---|
| 1126 | m(3,0) = m(3,1) = m(3,2) = m(0,3) = m(1,3) = m(2,3) = GLH_ZERO;
|
---|
| 1127 | m(3,3) = GLH_ONE;
|
---|
| 1128 | }
|
---|
| 1129 |
|
---|
| 1130 | quaternion & set_value( const real * qp )
|
---|
| 1131 | {
|
---|
| 1132 | memcpy(q,qp,sizeof(real) * 4);
|
---|
| 1133 |
|
---|
| 1134 | counter = 0;
|
---|
| 1135 | return *this;
|
---|
| 1136 | }
|
---|
| 1137 |
|
---|
| 1138 | quaternion & set_value( const matrix4 & m )
|
---|
| 1139 | {
|
---|
| 1140 | real tr, s;
|
---|
| 1141 | int i, j, k;
|
---|
| 1142 | const int nxt[3] = { 1, 2, 0 };
|
---|
| 1143 |
|
---|
| 1144 | tr = m(0,0) + m(1,1) + m(2,2);
|
---|
| 1145 |
|
---|
| 1146 | if ( tr > GLH_ZERO )
|
---|
| 1147 | {
|
---|
| 1148 | s = real(sqrt( tr + m(3,3) ));
|
---|
| 1149 | q[3] = real ( s * 0.5 );
|
---|
| 1150 | s = real(0.5) / s;
|
---|
| 1151 |
|
---|
| 1152 | q[0] = real ( ( m(1,2) - m(2,1) ) * s );
|
---|
| 1153 | q[1] = real ( ( m(2,0) - m(0,2) ) * s );
|
---|
| 1154 | q[2] = real ( ( m(0,1) - m(1,0) ) * s );
|
---|
| 1155 | }
|
---|
| 1156 | else
|
---|
| 1157 | {
|
---|
| 1158 | i = 0;
|
---|
| 1159 | if ( m(1,1) > m(0,0) )
|
---|
| 1160 | i = 1;
|
---|
| 1161 |
|
---|
| 1162 | if ( m(2,2) > m(i,i) )
|
---|
| 1163 | i = 2;
|
---|
| 1164 |
|
---|
| 1165 | j = nxt[i];
|
---|
| 1166 | k = nxt[j];
|
---|
| 1167 |
|
---|
| 1168 | s = real(sqrt( ( m(i,j) - ( m(j,j) + m(k,k) )) + GLH_ONE ));
|
---|
| 1169 |
|
---|
| 1170 | q[i] = real ( s * 0.5 );
|
---|
| 1171 | s = real(0.5 / s);
|
---|
| 1172 |
|
---|
| 1173 | q[3] = real ( ( m(j,k) - m(k,j) ) * s );
|
---|
| 1174 | q[j] = real ( ( m(i,j) + m(j,i) ) * s );
|
---|
| 1175 | q[k] = real ( ( m(i,k) + m(k,i) ) * s );
|
---|
| 1176 | }
|
---|
| 1177 |
|
---|
| 1178 | counter = 0;
|
---|
| 1179 | return *this;
|
---|
| 1180 | }
|
---|
| 1181 |
|
---|
| 1182 | quaternion & set_value( const vec3 &axis, real theta )
|
---|
| 1183 | {
|
---|
| 1184 | real sqnorm = axis.square_norm();
|
---|
| 1185 |
|
---|
| 1186 | if (sqnorm <= GLH_EPSILON)
|
---|
| 1187 | {
|
---|
| 1188 | // axis too small.
|
---|
| 1189 | x = y = z = 0.0;
|
---|
| 1190 | w = 1.0;
|
---|
| 1191 | }
|
---|
| 1192 | else
|
---|
| 1193 | {
|
---|
| 1194 | theta *= real(0.5);
|
---|
| 1195 | real sin_theta = real(sin(theta));
|
---|
| 1196 |
|
---|
| 1197 | if (!equivalent(sqnorm,GLH_ONE))
|
---|
| 1198 | sin_theta /= real(sqrt(sqnorm));
|
---|
| 1199 | x = sin_theta * axis.v[0];
|
---|
| 1200 | y = sin_theta * axis.v[1];
|
---|
| 1201 | z = sin_theta * axis.v[2];
|
---|
| 1202 | w = real(cos(theta));
|
---|
| 1203 | }
|
---|
| 1204 | return *this;
|
---|
| 1205 | }
|
---|
| 1206 |
|
---|
| 1207 | quaternion & set_value( const vec3 & rotateFrom, const vec3 & rotateTo )
|
---|
| 1208 | {
|
---|
| 1209 | vec3 p1, p2;
|
---|
| 1210 | real alpha;
|
---|
| 1211 |
|
---|
| 1212 | p1 = rotateFrom;
|
---|
| 1213 | p1.normalize();
|
---|
| 1214 | p2 = rotateTo;
|
---|
| 1215 | p2.normalize();
|
---|
| 1216 |
|
---|
| 1217 | alpha = p1.dot(p2);
|
---|
| 1218 |
|
---|
| 1219 | if(equivalent(alpha,GLH_ONE))
|
---|
| 1220 | {
|
---|
| 1221 | *this = identity();
|
---|
| 1222 | return *this;
|
---|
| 1223 | }
|
---|
| 1224 |
|
---|
| 1225 | // ensures that the anti-parallel case leads to a positive dot
|
---|
| 1226 | if(equivalent(alpha,-GLH_ONE))
|
---|
| 1227 | {
|
---|
| 1228 | vec3 v;
|
---|
| 1229 |
|
---|
| 1230 | if(p1.v[0] != p1.v[1] || p1.v[0] != p1.v[2])
|
---|
| 1231 | v = vec3(p1.v[1], p1.v[2], p1.v[0]);
|
---|
| 1232 | else
|
---|
| 1233 | v = vec3(-p1.v[0], p1.v[1], p1.v[2]);
|
---|
| 1234 |
|
---|
| 1235 | v -= p1 * p1.dot(v);
|
---|
| 1236 | v.normalize();
|
---|
| 1237 |
|
---|
| 1238 | set_value(v, GLH_PI);
|
---|
| 1239 | return *this;
|
---|
| 1240 | }
|
---|
| 1241 |
|
---|
| 1242 | p1 = p1.cross(p2);
|
---|
| 1243 | p1.normalize();
|
---|
| 1244 | set_value(p1,real(acos(alpha)));
|
---|
| 1245 |
|
---|
| 1246 | counter = 0;
|
---|
| 1247 | return *this;
|
---|
| 1248 | }
|
---|
| 1249 |
|
---|
| 1250 | quaternion & set_value( const vec3 & from_look, const vec3 & from_up,
|
---|
| 1251 | const vec3 & to_look, const vec3 & to_up)
|
---|
| 1252 | {
|
---|
| 1253 | quaternion r_look = quaternion(from_look, to_look);
|
---|
| 1254 |
|
---|
| 1255 | vec3 rotated_from_up(from_up);
|
---|
| 1256 | r_look.mult_vec(rotated_from_up);
|
---|
| 1257 |
|
---|
| 1258 | quaternion r_twist = quaternion(rotated_from_up, to_up);
|
---|
| 1259 |
|
---|
| 1260 | *this = r_twist;
|
---|
| 1261 | *this *= r_look;
|
---|
| 1262 | return *this;
|
---|
| 1263 | }
|
---|
| 1264 |
|
---|
| 1265 | quaternion & operator *= ( const quaternion & qr )
|
---|
| 1266 | {
|
---|
| 1267 | quaternion ql(*this);
|
---|
| 1268 |
|
---|
| 1269 | w = ql.w * qr.w - ql.x * qr.x - ql.y * qr.y - ql.z * qr.z;
|
---|
| 1270 | x = ql.w * qr.x + ql.x * qr.w + ql.y * qr.z - ql.z * qr.y;
|
---|
| 1271 | y = ql.w * qr.y + ql.y * qr.w + ql.z * qr.x - ql.x * qr.z;
|
---|
| 1272 | z = ql.w * qr.z + ql.z * qr.w + ql.x * qr.y - ql.y * qr.x;
|
---|
| 1273 |
|
---|
| 1274 | counter += qr.counter;
|
---|
| 1275 | counter++;
|
---|
| 1276 | counter_normalize();
|
---|
| 1277 | return *this;
|
---|
| 1278 | }
|
---|
| 1279 |
|
---|
| 1280 | void normalize()
|
---|
| 1281 | {
|
---|
| 1282 | real rnorm = GLH_ONE / real(sqrt(w * w + x * x + y * y + z * z));
|
---|
| 1283 | if (equivalent(rnorm, GLH_ZERO))
|
---|
| 1284 | return;
|
---|
| 1285 | x *= rnorm;
|
---|
| 1286 | y *= rnorm;
|
---|
| 1287 | z *= rnorm;
|
---|
| 1288 | w *= rnorm;
|
---|
| 1289 | counter = 0;
|
---|
| 1290 | }
|
---|
| 1291 |
|
---|
| 1292 | friend bool operator == ( const quaternion & q1, const quaternion & q2 );
|
---|
| 1293 |
|
---|
| 1294 | friend bool operator != ( const quaternion & q1, const quaternion & q2 );
|
---|
| 1295 |
|
---|
| 1296 | friend quaternion operator * ( const quaternion & q1, const quaternion & q2 );
|
---|
| 1297 |
|
---|
| 1298 | bool equals( const quaternion & r, real tolerance ) const
|
---|
| 1299 | {
|
---|
| 1300 | real t;
|
---|
| 1301 |
|
---|
| 1302 | t = (
|
---|
| 1303 | (q[0]-r.q[0])*(q[0]-r.q[0]) +
|
---|
| 1304 | (q[1]-r.q[1])*(q[1]-r.q[1]) +
|
---|
| 1305 | (q[2]-r.q[2])*(q[2]-r.q[2]) +
|
---|
| 1306 | (q[3]-r.q[3])*(q[3]-r.q[3]) );
|
---|
| 1307 | if(t > GLH_EPSILON)
|
---|
| 1308 | return false;
|
---|
| 1309 | return 1;
|
---|
| 1310 | }
|
---|
| 1311 |
|
---|
| 1312 | quaternion & conjugate()
|
---|
| 1313 | {
|
---|
| 1314 | q[0] *= -GLH_ONE;
|
---|
| 1315 | q[1] *= -GLH_ONE;
|
---|
| 1316 | q[2] *= -GLH_ONE;
|
---|
| 1317 | return *this;
|
---|
| 1318 | }
|
---|
| 1319 |
|
---|
| 1320 | quaternion & invert()
|
---|
| 1321 | {
|
---|
| 1322 | return conjugate();
|
---|
| 1323 | }
|
---|
| 1324 |
|
---|
| 1325 | quaternion inverse() const
|
---|
| 1326 | {
|
---|
| 1327 | quaternion r = *this;
|
---|
| 1328 | return r.invert();
|
---|
| 1329 | }
|
---|
| 1330 |
|
---|
| 1331 | //
|
---|
| 1332 | // Quaternion multiplication with cartesian vector
|
---|
| 1333 | // v' = q*v*q(star)
|
---|
| 1334 | //
|
---|
| 1335 | void mult_vec( const vec3 &src, vec3 &dst ) const
|
---|
| 1336 | {
|
---|
| 1337 | real v_coef = w * w - x * x - y * y - z * z;
|
---|
| 1338 | real u_coef = GLH_TWO * (src.v[0] * x + src.v[1] * y + src.v[2] * z);
|
---|
| 1339 | real c_coef = GLH_TWO * w;
|
---|
| 1340 |
|
---|
| 1341 | dst.v[0] = v_coef * src.v[0] + u_coef * x + c_coef * (y * src.v[2] - z * src.v[1]);
|
---|
| 1342 | dst.v[1] = v_coef * src.v[1] + u_coef * y + c_coef * (z * src.v[0] - x * src.v[2]);
|
---|
| 1343 | dst.v[2] = v_coef * src.v[2] + u_coef * z + c_coef * (x * src.v[1] - y * src.v[0]);
|
---|
| 1344 | }
|
---|
| 1345 |
|
---|
| 1346 | void mult_vec( vec3 & src_and_dst) const
|
---|
| 1347 | {
|
---|
| 1348 | mult_vec(vec3(src_and_dst), src_and_dst);
|
---|
| 1349 | }
|
---|
| 1350 |
|
---|
| 1351 | void scale_angle( real scaleFactor )
|
---|
| 1352 | {
|
---|
| 1353 | vec3 axis;
|
---|
| 1354 | real radians;
|
---|
| 1355 |
|
---|
| 1356 | get_value(axis, radians);
|
---|
| 1357 | radians *= scaleFactor;
|
---|
| 1358 | set_value(axis, radians);
|
---|
| 1359 | }
|
---|
| 1360 |
|
---|
| 1361 | static quaternion slerp( const quaternion & p, const quaternion & q, real alpha )
|
---|
| 1362 | {
|
---|
| 1363 | quaternion r;
|
---|
| 1364 |
|
---|
| 1365 | real cos_omega = p.x * q.x + p.y * q.y + p.z * q.z + p.w * q.w;
|
---|
| 1366 | // if B is on opposite hemisphere from A, use -B instead
|
---|
| 1367 |
|
---|
| 1368 | int bflip;
|
---|
| 1369 | if ( ( bflip = (cos_omega < GLH_ZERO)) )
|
---|
| 1370 | cos_omega = -cos_omega;
|
---|
| 1371 |
|
---|
| 1372 | // complementary interpolation parameter
|
---|
| 1373 | real beta = GLH_ONE - alpha;
|
---|
| 1374 |
|
---|
| 1375 | if(cos_omega >= GLH_ONE - GLH_EPSILON)
|
---|
| 1376 | return p;
|
---|
| 1377 |
|
---|
| 1378 | real omega = real(acos(cos_omega));
|
---|
| 1379 | real one_over_sin_omega = GLH_ONE / real(sin(omega));
|
---|
| 1380 |
|
---|
| 1381 | beta = real(sin(omega*beta) * one_over_sin_omega);
|
---|
| 1382 | alpha = real(sin(omega*alpha) * one_over_sin_omega);
|
---|
| 1383 |
|
---|
| 1384 | if (bflip)
|
---|
| 1385 | alpha = -alpha;
|
---|
| 1386 |
|
---|
| 1387 | r.x = beta * p.q[0]+ alpha * q.q[0];
|
---|
| 1388 | r.y = beta * p.q[1]+ alpha * q.q[1];
|
---|
| 1389 | r.z = beta * p.q[2]+ alpha * q.q[2];
|
---|
| 1390 | r.w = beta * p.q[3]+ alpha * q.q[3];
|
---|
| 1391 | return r;
|
---|
| 1392 | }
|
---|
| 1393 |
|
---|
| 1394 | static quaternion identity()
|
---|
| 1395 | {
|
---|
| 1396 | static quaternion ident( vec3( 0.0, 0.0, 0.0 ), GLH_ONE );
|
---|
| 1397 | return ident;
|
---|
| 1398 | }
|
---|
| 1399 |
|
---|
| 1400 | real & operator []( int i )
|
---|
| 1401 | {
|
---|
| 1402 | assert(i < 4);
|
---|
| 1403 | return q[i];
|
---|
| 1404 | }
|
---|
| 1405 |
|
---|
| 1406 | const real & operator []( int i ) const
|
---|
| 1407 | {
|
---|
| 1408 | assert(i < 4);
|
---|
| 1409 | return q[i];
|
---|
| 1410 | }
|
---|
| 1411 |
|
---|
| 1412 | protected:
|
---|
| 1413 |
|
---|
| 1414 | void counter_normalize()
|
---|
| 1415 | {
|
---|
| 1416 | if (counter > GLH_QUATERNION_NORMALIZATION_THRESHOLD)
|
---|
| 1417 | normalize();
|
---|
| 1418 | }
|
---|
| 1419 |
|
---|
| 1420 | union
|
---|
| 1421 | {
|
---|
| 1422 | struct
|
---|
| 1423 | {
|
---|
| 1424 | real q[4];
|
---|
| 1425 | };
|
---|
| 1426 | struct
|
---|
| 1427 | {
|
---|
| 1428 | real x;
|
---|
| 1429 | real y;
|
---|
| 1430 | real z;
|
---|
| 1431 | real w;
|
---|
| 1432 | };
|
---|
| 1433 | };
|
---|
| 1434 |
|
---|
| 1435 | // renormalization counter
|
---|
| 1436 | unsigned char counter;
|
---|
| 1437 | };
|
---|
| 1438 |
|
---|
| 1439 | inline
|
---|
| 1440 | bool operator == ( const quaternion & q1, const quaternion & q2 )
|
---|
| 1441 | {
|
---|
| 1442 | return (equivalent(q1.x, q2.x) &&
|
---|
| 1443 | equivalent(q1.y, q2.y) &&
|
---|
| 1444 | equivalent(q1.z, q2.z) &&
|
---|
| 1445 | equivalent(q1.w, q2.w) );
|
---|
| 1446 | }
|
---|
| 1447 |
|
---|
| 1448 | inline
|
---|
| 1449 | bool operator != ( const quaternion & q1, const quaternion & q2 )
|
---|
| 1450 | {
|
---|
| 1451 | return ! ( q1 == q2 );
|
---|
| 1452 | }
|
---|
| 1453 |
|
---|
| 1454 | inline
|
---|
| 1455 | quaternion operator * ( const quaternion & q1, const quaternion & q2 )
|
---|
| 1456 | {
|
---|
| 1457 | quaternion r(q1);
|
---|
| 1458 | r *= q2;
|
---|
| 1459 | return r;
|
---|
| 1460 | }
|
---|
| 1461 |
|
---|
| 1462 |
|
---|
| 1463 |
|
---|
| 1464 |
|
---|
| 1465 |
|
---|
| 1466 |
|
---|
| 1467 |
|
---|
| 1468 |
|
---|
| 1469 |
|
---|
| 1470 |
|
---|
| 1471 | class plane
|
---|
| 1472 | {
|
---|
| 1473 | public:
|
---|
| 1474 |
|
---|
| 1475 | plane()
|
---|
| 1476 | {
|
---|
| 1477 | planedistance = 0.0;
|
---|
| 1478 | planenormal.set_value( 0.0, 0.0, 1.0 );
|
---|
| 1479 | }
|
---|
| 1480 |
|
---|
| 1481 |
|
---|
| 1482 | plane( const vec3 &p0, const vec3 &p1, const vec3 &p2 )
|
---|
| 1483 | {
|
---|
| 1484 | vec3 v0 = p1 - p0;
|
---|
| 1485 | vec3 v1 = p2 - p0;
|
---|
| 1486 | planenormal = v0.cross(v1);
|
---|
| 1487 | planenormal.normalize();
|
---|
| 1488 | planedistance = p0.dot(planenormal);
|
---|
| 1489 | }
|
---|
| 1490 |
|
---|
| 1491 | plane( const vec3 &normal, real distance )
|
---|
| 1492 | {
|
---|
| 1493 | planedistance = distance;
|
---|
| 1494 | planenormal = normal;
|
---|
| 1495 | planenormal.normalize();
|
---|
| 1496 | }
|
---|
| 1497 |
|
---|
| 1498 | plane( const vec3 &normal, const vec3 &point )
|
---|
| 1499 | {
|
---|
| 1500 | planenormal = normal;
|
---|
| 1501 | planenormal.normalize();
|
---|
| 1502 | planedistance = point.dot(planenormal);
|
---|
| 1503 | }
|
---|
| 1504 |
|
---|
| 1505 | void offset( real d )
|
---|
| 1506 | {
|
---|
| 1507 | planedistance += d;
|
---|
| 1508 | }
|
---|
| 1509 |
|
---|
| 1510 | bool intersect( const line &l, vec3 &intersection ) const
|
---|
| 1511 | {
|
---|
| 1512 | vec3 pos, dir;
|
---|
| 1513 | vec3 pn = planenormal;
|
---|
| 1514 | real pd = planedistance;
|
---|
| 1515 |
|
---|
| 1516 | pos = l.get_position();
|
---|
| 1517 | dir = l.get_direction();
|
---|
| 1518 |
|
---|
| 1519 | if(dir.dot(pn) == 0.0) return 0;
|
---|
| 1520 | pos -= pn*pd;
|
---|
| 1521 | // now we're talking about a plane passing through the origin
|
---|
| 1522 | if(pos.dot(pn) < 0.0) pn.negate();
|
---|
| 1523 | if(dir.dot(pn) > 0.0) dir.negate();
|
---|
| 1524 | vec3 ppos = pn * pos.dot(pn);
|
---|
| 1525 | pos = (ppos.length()/dir.dot(-pn))*dir;
|
---|
| 1526 | intersection = l.get_position();
|
---|
| 1527 | intersection += pos;
|
---|
| 1528 | return 1;
|
---|
| 1529 | }
|
---|
| 1530 | void transform( const matrix4 &matrix )
|
---|
| 1531 | {
|
---|
| 1532 | matrix4 invtr = matrix.inverse();
|
---|
| 1533 | invtr = invtr.transpose();
|
---|
| 1534 |
|
---|
| 1535 | vec3 pntOnplane = planenormal * planedistance;
|
---|
| 1536 | vec3 newPntOnplane;
|
---|
| 1537 | vec3 newnormal;
|
---|
| 1538 |
|
---|
| 1539 | invtr.mult_dir_matrix(planenormal, newnormal);
|
---|
| 1540 | matrix.mult_vec_matrix(pntOnplane, newPntOnplane);
|
---|
| 1541 |
|
---|
| 1542 | newnormal.normalize();
|
---|
| 1543 | planenormal = newnormal;
|
---|
| 1544 | planedistance = newPntOnplane.dot(planenormal);
|
---|
| 1545 | }
|
---|
| 1546 |
|
---|
| 1547 | bool is_in_half_space( const vec3 &point ) const
|
---|
| 1548 | {
|
---|
| 1549 |
|
---|
| 1550 | if(( point.dot(planenormal) - planedistance) < 0.0)
|
---|
| 1551 | return 0;
|
---|
| 1552 | return 1;
|
---|
| 1553 | }
|
---|
| 1554 |
|
---|
| 1555 |
|
---|
| 1556 | real distance( const vec3 & point ) const
|
---|
| 1557 | {
|
---|
| 1558 | return planenormal.dot(point - planenormal*planedistance);
|
---|
| 1559 | }
|
---|
| 1560 |
|
---|
| 1561 | const vec3 &get_normal() const
|
---|
| 1562 | {
|
---|
| 1563 | return planenormal;
|
---|
| 1564 | }
|
---|
| 1565 |
|
---|
| 1566 |
|
---|
| 1567 | real get_distance_from_origin() const
|
---|
| 1568 | {
|
---|
| 1569 | return planedistance;
|
---|
| 1570 | }
|
---|
| 1571 |
|
---|
| 1572 |
|
---|
| 1573 | friend bool operator == ( const plane & p1, const plane & p2 );
|
---|
| 1574 |
|
---|
| 1575 |
|
---|
| 1576 | friend bool operator != ( const plane & p1, const plane & p2 );
|
---|
| 1577 |
|
---|
| 1578 | //protected:
|
---|
| 1579 | vec3 planenormal;
|
---|
| 1580 | real planedistance;
|
---|
| 1581 | };
|
---|
| 1582 |
|
---|
| 1583 | inline
|
---|
| 1584 | bool operator == (const plane & p1, const plane & p2 )
|
---|
| 1585 | {
|
---|
| 1586 | return ( p1.planedistance == p2.planedistance && p1.planenormal == p2.planenormal);
|
---|
| 1587 | }
|
---|
| 1588 |
|
---|
| 1589 | inline
|
---|
| 1590 | bool operator != ( const plane & p1, const plane & p2 )
|
---|
| 1591 | { return ! (p1 == p2); }
|
---|
| 1592 |
|
---|
| 1593 |
|
---|
| 1594 |
|
---|
| 1595 | } // "ns_##GLH_REAL"
|
---|
| 1596 |
|
---|
| 1597 | // make common typedefs...
|
---|
| 1598 | #ifdef GLH_REAL_IS_FLOAT
|
---|
| 1599 | typedef GLH_REAL_NAMESPACE::vec2 vec2f;
|
---|
| 1600 | typedef GLH_REAL_NAMESPACE::vec3 vec3f;
|
---|
| 1601 | typedef GLH_REAL_NAMESPACE::vec4 vec4f;
|
---|
| 1602 | typedef GLH_REAL_NAMESPACE::quaternion quaternionf;
|
---|
| 1603 | typedef GLH_REAL_NAMESPACE::quaternion rotationf;
|
---|
| 1604 | typedef GLH_REAL_NAMESPACE::line linef;
|
---|
| 1605 | typedef GLH_REAL_NAMESPACE::plane planef;
|
---|
| 1606 | typedef GLH_REAL_NAMESPACE::matrix4 matrix4f;
|
---|
| 1607 | #endif
|
---|
| 1608 |
|
---|
| 1609 |
|
---|
| 1610 |
|
---|
| 1611 |
|
---|
| 1612 | } // namespace glh
|
---|
| 1613 |
|
---|
| 1614 |
|
---|
| 1615 |
|
---|
| 1616 | #endif
|
---|
| 1617 |
|
---|