src/subpic/CoordGeom.cpp

   1 /*
   2  *      Copyright (C) 2003-2006 Gabest
   3  *      http://www.gabest.org
   4  *
   5  *  This Program is free software; you can redistribute it and/or modify
   6  *  it under the terms of the GNU General Public License as published by
   7  *  the Free Software Foundation; either version 2, or (at your option)
   8  *  any later version.
   9  *
  10  *  This Program is distributed in the hope that it will be useful,
  11  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  13  *  GNU General Public License for more details.
  14  *
  15  *  You should have received a copy of the GNU General Public License
  16  *  along with GNU Make; see the file COPYING.  If not, write to
  17  *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
  18  *  http://www.gnu.org/copyleft/gpl.html
  19  *
  20  */
  21
  22 #include "stdafx.h"
  23 #include <math.h>
  24 #include "CoordGeom.h"
  25
  26 #define EPSILON (1e-7)
  27 #define BIGNUMBER (1e+9)
  28 #define IsZero(d) (fabs(d) < EPSILON)
  29
  30 //
  31 // Vector
  32 //
  33
  34 Vector::Vector(float x, float y, float z)
  35 {
  36         this->x = x;
  37         this->y = y;
  38         this->z = z;
  39 }
  40
  41 void Vector::Set(float x, float y, float z)
  42 {
  43         this->x = x;
  44         this->y = y;
  45         this->z = z;
  46 }
  47
  48 float Vector::Length()
  49 {
  50         return(sqrt(x * x + y * y + z * z));
  51 }
  52
  53 float Vector::Sum()
  54 {
  55         return(x + y + z);
  56 }
  57
  58 float Vector::CrossSum()
  59 {
  60         return(x*y + x*z + y*z);
  61 }
  62
  63 Vector Vector::Cross()
  64 {
  65         return(Vector(x*y, x*z, y*z));
  66 }
  67
  68 Vector Vector::Pow(float exp)
  69 {
  70         return(exp == 0 ? Vector(1, 1, 1) : exp == 1 ? *this : Vector(pow(x, exp), pow(y, exp), pow(z, exp)));
  71 }
  72
  73 Vector Vector::Unit()
  74 {
  75         float l = Length();
  76         if(!l || l == 1) return(*this);
  77         return(*this * (1 / l));
  78 }
  79
  80 Vector& Vector::Unitalize()
  81 {
  82         return(*this = Unit());
  83 }
  84
  85 Vector Vector::Normal(Vector& a, Vector& b)
  86 {
  87         return((a - *this) % (b - a));
  88 }
  89
  90 float Vector::Angle(Vector& a, Vector& b)
  91 {
  92         return(((a - *this).Unit()).Angle((b - *this).Unit()));
  93 }
  94
  95 float Vector::Angle(Vector& a)
  96 {
  97         float angle = *this | a;
  98         return((angle > 1) ? 0 : (angle < -1) ? PI : acos(angle));
  99 }
 100
 101 void Vector::Angle(float& u, float& v)
 102 {
 103         Vector n = Unit();
 104
 105         u = asin(n.y);
 106
 107         if(IsZero(n.z)) v = PI/2 * Sgn(n.x);
 108         else if(n.z > 0) v = atan(n.x / n.z);
 109         else if(n.z < 0) v = IsZero(n.x) ? PI : (PI * Sgn(n.x) + atan(n.x / n.z));
 110 }
 111
 112 Vector Vector::Angle()
 113 {
 114         Vector ret;
 115         Angle(ret.x, ret.y);
 116         ret.z = 0;
 117         return(ret);
 118 }
 119
 120 Vector& Vector::Min(Vector& a)
 121 {
 122         x = (x < a.x) ? x : a.x;
 123         y = (y < a.y) ? y : a.y;
 124         z = (z < a.z) ? z : a.z;
 125         return(*this);
 126 }
 127
 128 Vector& Vector::Max(Vector& a)
 129 {
 130         x = (x > a.x) ? x : a.x;
 131         y = (y > a.y) ? y : a.y;
 132         z = (z > a.z) ? z : a.z;
 133         return(*this);
 134 }
 135
 136 Vector Vector::Abs()
 137 {
 138         return(Vector(fabs(x), fabs(y), fabs(z)));
 139 }
 140
 141 Vector Vector::Reflect(Vector& n)
 142 {
 143         return(n * ((-*this) | n) * 2 - (-*this));
 144 }
 145
 146 Vector Vector::Refract(Vector& N, float nFront, float nBack, float* nOut)
 147 {
 148         Vector D = -*this;
 149
 150         float N_dot_D = (N | D);
 151         float n = N_dot_D >= 0 ? (nFront / nBack) : (nBack / nFront);
 152
 153         Vector cos_D = N * N_dot_D;
 154         Vector sin_T = (cos_D - D) * n;
 155
 156         float len_sin_T = sin_T | sin_T;
 157
 158         if(len_sin_T > 1)
 159         {
 160                 if(nOut) {*nOut = N_dot_D >= 0 ? nFront : nBack;}
 161                 return((*this).Reflect(N));
 162         }
 163
 164         float N_dot_T = sqrt(1.0 - len_sin_T);
 165         if(N_dot_D < 0) N_dot_T = -N_dot_T;
 166
 167         if(nOut) {*nOut = N_dot_D >= 0 ? nBack : nFront;}
 168
 169         return(sin_T - (N * N_dot_T));
 170 }
 171
 172 Vector Vector::Refract2(Vector& N, float nFrom, float nTo, float* nOut)
 173 {
 174         Vector D = -*this;
 175
 176         float N_dot_D = (N | D);
 177         float n = nFrom / nTo;
 178
 179         Vector cos_D = N * N_dot_D;
 180         Vector sin_T = (cos_D - D) * n;
 181
 182         float len_sin_T = sin_T | sin_T;
 183
 184         if(len_sin_T > 1)
 185         {
 186                 if(nOut) {*nOut = nFrom;}
 187                 return((*this).Reflect(N));
 188         }
 189
 190         float N_dot_T = sqrt(1.0 - len_sin_T);
 191         if(N_dot_D < 0) N_dot_T = -N_dot_T;
 192
 193         if(nOut) {*nOut = nTo;}
 194
 195         return(sin_T - (N * N_dot_T));
 196 }
 197
 198 float Vector::operator | (Vector& v)
 199 {
 200         return(x * v.x + y * v.y + z * v.z);
 201 }
 202
 203 Vector Vector::operator % (Vector& v)
 204 {
 205         return(Vector(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x));
 206 }
 207
 208 float& Vector::operator [] (int i)
 209 {
 210         return(!i ? x : (i == 1) ? y : z);
 211 }
 212
 213 Vector Vector::operator - ()
 214 {
 215         return(Vector(-x, -y, -z));
 216 }
 217
 218 bool Vector::operator == (const Vector& v) const
 219 {
 220         if(IsZero(x - v.x) && IsZero(y - v.y) && IsZero(z - v.z)) return(true);
 221         return(false);
 222 }
 223
 224 bool Vector::operator != (const Vector& v) const
 225 {
 226         return((*this == v) ? false : true);
 227 }
 228
 229 Vector Vector::operator + (float d)
 230 {
 231         return(Vector(x + d, y + d, z + d));
 232 }
 233
 234 Vector Vector::operator + (Vector& v)
 235 {
 236         return(Vector(x + v.x, y + v.y, z + v.z));
 237 }
 238
 239 Vector Vector::operator - (float d)
 240 {
 241         return(Vector(x - d, y - d, z - d));
 242 }
 243
 244 Vector Vector::operator - (Vector& v)
 245 {
 246         return(Vector(x - v.x, y - v.y, z - v.z));
 247 }
 248
 249 Vector Vector::operator * (float d)
 250 {
 251         return(Vector(x * d, y * d, z * d));
 252 }
 253
 254 Vector Vector::operator * (Vector& v)
 255 {
 256         return(Vector(x * v.x, y * v.y, z * v.z));
 257 }
 258
 259 Vector Vector::operator / (float d)
 260 {
 261         return(Vector(x / d, y / d, z / d));
 262 }
 263
 264 Vector Vector::operator / (Vector& v)
 265 {
 266         return(Vector(x / v.x, y / v.y, z / v.z));
 267 }
 268
 269 Vector& Vector::operator += (float d)
 270 {
 271         x += d; y += d; z += d;
 272         return(*this);
 273 }
 274
 275 Vector& Vector::operator += (Vector& v)
 276 {
 277         x += v.x; y += v.y; z += v.z;
 278         return(*this);
 279 }
 280
 281 Vector& Vector::operator -= (float d)
 282 {
 283         x -= d; y -= d; z -= d;
 284         return(*this);
 285 }
 286
 287 Vector& Vector::operator -= (Vector& v)
 288 {
 289         x -= v.x; y -= v.y; z -= v.z;
 290         return(*this);
 291 }
 292
 293 Vector& Vector::operator *= (float d)
 294 {
 295         x *= d; y *= d; z *= d;
 296         return(*this);
 297 }
 298
 299 Vector& Vector::operator *= (Vector& v)
 300 {
 301         x *= v.x; y *= v.y; z *= v.z;
 302         return(*this);
 303 }
 304
 305 Vector& Vector::operator /= (float d)
 306 {
 307         x /= d; y /= d; z /= d;
 308         return(*this);
 309 }
 310
 311 Vector& Vector::operator /= (Vector& v)
 312 {
 313         x /= v.x; y /= v.y; z /= v.z;
 314         return(*this);
 315 }
 316
 317 //
 318 // Ray
 319 //
 320
 321 Ray::Ray(Vector& p, Vector& d)
 322 {
 323         this->p = p;
 324         this->d = d;
 325 }
 326
 327 void Ray::Set(Vector& p, Vector& d)
 328 {
 329         this->p = p;
 330         this->d = d;
 331 }
 332
 333 float Ray::GetDistanceFrom(Ray& r)
 334 {
 335         float t = (d | r.d);
 336         if(IsZero(t)) return(-BIGNUMBER); // plane is paralell to the ray, return -infinite
 337         return(((r.p - p) | r.d) / t);
 338 }
 339
 340 float Ray::GetDistanceFrom(Vector& v)
 341 {
 342         float t = ((v - p) | d) / (d | d);
 343         return(((p + d*t) - v).Length());
 344 }
 345
 346 Vector Ray::operator [] (float t)
 347 {
 348         return(p + d*t);
 349 }
 350
 351 //
 352 // XForm
 353 //
 354
 355 XForm::XForm(Ray& r, Vector& s, bool isWorldToLocal)
 356 {
 357         Initalize(r, s, isWorldToLocal);
 358 }
 359
 360 void XForm::Initalize()
 361 {
 362         m.Initalize();
 363 }
 364
 365 void XForm::Initalize(Ray& r, Vector& s, bool isWorldToLocal)
 366 {
 367         Initalize();
 368
 369         if(m_isWorldToLocal = isWorldToLocal)
 370         {
 371                 *this -= r.p;
 372                 *this >>= r.d;
 373                 *this /= s;
 374
 375         }
 376         else
 377         {
 378                 *this *= s;
 379                 *this <<= r.d;
 380                 *this += r.p;
 381         }
 382 }
 383
 384 void XForm::operator *= (Vector& v)
 385 {
 386         Matrix s;
 387         s.mat[0][0] = v.x;
 388         s.mat[1][1] = v.y;
 389         s.mat[2][2] = v.z;
 390         m *= s;
 391 }
 392
 393 void XForm::operator += (Vector& v)
 394 {
 395         Matrix t;
 396         t.mat[3][0] = v.x;
 397         t.mat[3][1] = v.y;
 398         t.mat[3][2] = v.z;
 399         m *= t;
 400 }
 401
 402 void XForm::operator <<= (Vector& v)
 403 {
 404         Matrix x;
 405         x.mat[1][1] = cos(v.x); x.mat[1][2] = -sin(v.x);
 406         x.mat[2][1] = sin(v.x); x.mat[2][2] = cos(v.x);
 407
 408         Matrix y;
 409         y.mat[0][0] = cos(v.y); y.mat[0][2] = -sin(v.y);
 410         y.mat[2][0] = sin(v.y); y.mat[2][2] = cos(v.y);
 411
 412         Matrix z;
 413         z.mat[0][0] = cos(v.z); z.mat[0][1] = -sin(v.z);
 414         z.mat[1][0] = sin(v.z); z.mat[1][1] = cos(v.z);
 415
 416         m = m_isWorldToLocal ? (m * y * x * z) : (m * z * x * y);
 417 }
 418
 419 void XForm::operator /= (Vector& v)
 420 {
 421         Vector s;
 422         s.x = IsZero(v.x) ? 0 : 1 / v.x;
 423         s.y = IsZero(v.y) ? 0 : 1 / v.y;
 424         s.z = IsZero(v.z) ? 0 : 1 / v.z;
 425         *this *= s;
 426 }
 427
 428 void XForm::operator -= (Vector& v)
 429 {
 430         *this += -v;
 431 }
 432
 433 void XForm::operator >>= (Vector& v)
 434 {
 435         *this <<= -v;
 436 }
 437
 438 Vector XForm::operator < (Vector& n)
 439 {
 440         Vector ret;
 441
 442         ret.x = n.x * m.mat[0][0] +
 443                         n.y * m.mat[1][0] +
 444                         n.z * m.mat[2][0];
 445         ret.y = n.x * m.mat[0][1] +
 446                         n.y * m.mat[1][1] +
 447                         n.z * m.mat[2][1];
 448         ret.z = n.x * m.mat[0][2] +
 449                         n.y * m.mat[1][2] +
 450                         n.z * m.mat[2][2];
 451
 452         return(ret);
 453 }
 454
 455 Vector XForm::operator << (Vector& v)
 456 {
 457         Vector ret;
 458
 459         ret.x = v.x * m.mat[0][0] +
 460                         v.y * m.mat[1][0] +
 461                         v.z * m.mat[2][0] +
 462                         m.mat[3][0];
 463         ret.y = v.x * m.mat[0][1] +
 464                         v.y * m.mat[1][1] +
 465                         v.z * m.mat[2][1] +
 466                         m.mat[3][1];
 467         ret.z = v.x * m.mat[0][2] +
 468                         v.y * m.mat[1][2] +
 469                         v.z * m.mat[2][2] +
 470                         m.mat[3][2];
 471
 472         return(ret);
 473 }
 474
 475 Ray XForm::operator << (Ray& r)
 476 {
 477         return(Ray(*this << r.p, *this < r.d));
 478 }
 479
 480 //
 481 // XForm::Matrix
 482 //
 483
 484 XForm::Matrix::Matrix()
 485 {
 486         Initalize();
 487 }
 488
 489 void XForm::Matrix::Initalize()
 490 {
 491         mat[0][0] = 1; mat[0][1] = 0; mat[0][2] = 0; mat[0][3] = 0;
 492         mat[1][0] = 0; mat[1][1] = 1; mat[1][2] = 0; mat[1][3] = 0;
 493         mat[2][0] = 0; mat[2][1] = 0; mat[2][2] = 1; mat[2][3] = 0;
 494         mat[3][0] = 0; mat[3][1] = 0; mat[3][2] = 0; mat[3][3] = 1;
 495 }
 496
 497 XForm::Matrix XForm::Matrix::operator * (Matrix& m)
 498 {
 499         Matrix ret;
 500
 501         for(int i = 0; i < 4; i++)
 502         {
 503                 for(int j = 0; j < 4; j++)
 504                 {
 505                         ret.mat[i][j] = mat[i][0] * m.mat[0][j] +
 506                                                         mat[i][1] * m.mat[1][j] +
 507                                                         mat[i][2] * m.mat[2][j] +
 508                                                         mat[i][3] * m.mat[3][j];
 509
 510                         if(IsZero(ret.mat[i][j])) ret.mat[i][j] = 0;
 511                 }
 512         }
 513
 514         return(ret);
 515 }
 516
 517 XForm::Matrix& XForm::Matrix::operator *= (XForm::Matrix& m)
 518 {
 519         return(*this = *this * m);
 520 }