git.mutantstargoat.com Git - summerhack/blob - src/3dengfx/src/n3dmath2/n3dmath2.cpp

   1 /*
   2 This file is part of the n3dmath2 library.
   3
   4 Copyright (c) 2004, 2005 John Tsiombikas <nuclear@siggraph.org>
   5
   6 The n3dmath2 library is free software; you can redistribute it and/or modify
   7 it under the terms of the GNU General Public License as published by
   8 the Free Software Foundation; either version 2 of the License, or
   9 (at your option) any later version.
  10
  11 The n3dmath2 library is distributed in the hope that it will be useful,
  12 but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 GNU General Public License for more details.
  15
  16 You should have received a copy of the GNU General Public License
  17 along with the n3dmath2 library; if not, write to the Free Software
  18 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  19 */
  20
  21 #include "n3dmath2.hpp"
  22
  23 const scalar_t e = 2.7182818;
  24
  25 const scalar_t pi = 3.1415926535897932;
  26 const scalar_t half_pi = pi / 2.0;
  27 const scalar_t quarter_pi = pi / 4.0;
  28 const scalar_t two_pi = pi * 2.0;
  29 const scalar_t three_half_pi = 3.0 * pi / 2.0;
  30
  31 const scalar_t xsmall_number = 1.e-8;
  32 const scalar_t small_number = 1.e-4;
  33
  34 const scalar_t error_margin = 1.e-6;
  35
  36
  37 scalar_t frand(scalar_t range) {
  38         return range * (float)rand() / (float)RAND_MAX;
  39 }
  40
  41 scalar_t integral(scalar_t (*f)(scalar_t), scalar_t low, scalar_t high, int samples)  {
  42         scalar_t h = (high - low) / (scalar_t)samples;
  43         scalar_t sum = 0.0;
  44
  45         for(int i=0; i<samples+1; i++) {
  46                 scalar_t y = f((scalar_t)i * h + low);
  47                 sum += ((!i || i == samples) ? y : ((i%2) ? 4.0 * y : 2.0 * y)) * (h / 3.0);
  48         }
  49         return sum;
  50 }
  51
  52 scalar_t gaussian(scalar_t x, scalar_t mean, scalar_t sdev) {
  53         scalar_t exponent = -SQ(x - mean) / (2.0 * SQ(sdev));
  54
  55         return 1.0 - -pow(e, exponent) / (sdev * sqrt(two_pi));
  56 }
  57
  58
  59 // -- b-spline approximation --
  60 scalar_t bspline(const Vector4 &cpvec, scalar_t t) {
  61         Matrix4x4 bspline_mat(  -1,  3, -3,  1,
  62                                                          3, -6,  3,  0,
  63                                                         -3,  0,  3,  0,
  64                                                          1,  4,  1,  0);
  65
  66         scalar_t t_square = t * t;
  67         scalar_t t_cube = t_square * t;
  68         Vector4 params(t_cube, t_square, t, 1.0);
  69
  70         return dot_product(params, cpvec.transformed(bspline_mat) / 6.0);
  71 }
  72
  73 // -- catmull rom spline interpolation --
  74 scalar_t catmull_rom_spline(const Vector4 &cpvec, scalar_t t) {
  75         Matrix4x4 crspline_mat( -1,  3, -3,  1,
  76                                                          2, -5,  4, -1,
  77                                                         -1,  0,  1,  0,
  78                                                          0,  2,  0,  0);
  79
  80         scalar_t t_square = t * t;
  81         scalar_t t_cube = t_square * t;
  82         Vector4 params(t_cube, t_square, t, 1.0);
  83
  84         return dot_product(params, cpvec.transformed(crspline_mat) / 2.0);
  85 }
  86
  87 /* Bezier - (MG)
  88  * returns a interpolated scalar value
  89  * given 4 control values
  90  */
  91 scalar_t bezier(const Vector4 &cp, scalar_t t)
  92 {
  93         static scalar_t omt, omt3, t3, f;
  94         t3 = t * t * t;
  95         omt = 1.0f - t;
  96         omt3 = omt * omt * omt;
  97         f = 3 * t * omt;
  98
  99         return (cp.x * omt3) + (cp.y * f * omt) + (cp.z * f * t) + (cp.w * t3);
 100 }
 101
 102 /* Bezier - (MG)
 103  * Vector3 overloaded bezier function
 104  */
 105 Vector3 bezier(const Vector3 &p0, const Vector3 &p1, const Vector3 &p2, const Vector3 &p3, scalar_t t)
 106 {
 107         static scalar_t omt, omt3, t3, f;
 108         t3 = t * t * t;
 109         omt = 1.0f - t;
 110         omt3 = omt * omt * omt;
 111         f = 3 * t * omt;
 112
 113         return (p0 * omt3) + (p1 * f * omt) + (p2 * f * t) + (p3 * t3);
 114 }
 115
 116 /* BezierTangent
 117  * returns a vector tangent to the
 118  * Bezier curve at the specified point
 119  */
 120 Vector3 bezier_tangent(const Vector3 &p0, const Vector3 &p1, const Vector3 &p2, const Vector3 &p3, scalar_t t)
 121 {
 122         static scalar_t omt;
 123         omt = 1.0f - t;
 124         static Vector3 p4, p5, p6, p7, p8;
 125         p4 = p0 * omt + p1 * t;
 126         p5 = p1 * omt + p2 * t;
 127         p6 = p2 * omt + p3 * t;
 128
 129         p7 = p4 * omt + p5 * t;
 130         p8 = p5 * omt + p6 * t;
 131
 132         return p8 - p7;
 133 }
 134
 135
 136 // -- point / line distance in 2D --
 137 scalar_t dist_line(const Vector2 &p1, const Vector2 &p2, const Vector2 &p) {
 138         if(p1.x == p2.x && p1.y == p2.y) return 0;      // avoid div/zero
 139         scalar_t t = dot_product(p - p1, p2 - p1) / (p2 - p1).length_sq();
 140
 141         Vector2 pt = p1 + (p2 - p1) * t;
 142         return (p - pt).length();
 143 }
 144
 145
 146 Basis::Basis() {
 147         i = Vector3(1, 0, 0);
 148         j = Vector3(0, 1, 0);
 149         k = Vector3(0, 0, 1);
 150 }
 151
 152 Basis::Basis(const Vector3 &i, const Vector3 &j, const Vector3 &k) {
 153         this->i = i;
 154         this->j = j;
 155         this->k = k;
 156 }
 157
 158 Basis::Basis(const Vector3 &dir, bool LeftHanded) {
 159         k = dir;
 160         j = VECTOR3_J;
 161         i = cross_product(j, k);
 162         j = cross_product(k, i);
 163 }
 164
 165
 166 void Basis::rotate(scalar_t x, scalar_t y, scalar_t z) {
 167         Matrix4x4 RotMat;
 168         RotMat.set_rotation(Vector3(x, y, z));
 169         i.transform(RotMat);
 170         j.transform(RotMat);
 171         k.transform(RotMat);
 172 }
 173
 174 void Basis::rotate(const Vector3 &axis, scalar_t angle) {
 175         Quaternion q;
 176         q.set_rotation(axis, angle);
 177         i.transform(q);
 178         j.transform(q);
 179         k.transform(q);
 180 }
 181
 182 void Basis::rotate(const Matrix4x4 &mat) {
 183         i.transform(mat);
 184         j.transform(mat);
 185         k.transform(mat);
 186 }
 187
 188 void Basis::rotate(const Quaternion &quat) {
 189         i.transform(quat);
 190         j.transform(quat);
 191         k.transform(quat);
 192 }
 193
 194 Matrix3x3 Basis::create_rotation_matrix() const {
 195         return Matrix3x3(       i.x, j.x, k.x,
 196                                                 i.y, j.y, k.y,
 197                                                 i.z, j.z, k.z);
 198 }
 199
 200
 201 //////////////
 202
 203 size_t size_of_scalar_type() {
 204         static const scalar_t temp = 0.0;
 205         return sizeof(temp);
 206 }
 207
 208