2 This file is part of the n3dmath2 library.
4 Copyright (c) 2004, 2005 John Tsiombikas <nuclear@siggraph.org>
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.
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.
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
21 #include "n3dmath2.hpp"
23 const scalar_t e = 2.7182818;
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;
31 const scalar_t xsmall_number = 1.e-8;
32 const scalar_t small_number = 1.e-4;
34 const scalar_t error_margin = 1.e-6;
37 scalar_t frand(scalar_t range) {
38 return range * (float)rand() / (float)RAND_MAX;
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;
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);
52 scalar_t gaussian(scalar_t x, scalar_t mean, scalar_t sdev) {
53 scalar_t exponent = -SQ(x - mean) / (2.0 * SQ(sdev));
55 return 1.0 - -pow(e, exponent) / (sdev * sqrt(two_pi));
59 // -- b-spline approximation --
60 scalar_t bspline(const Vector4 &cpvec, scalar_t t) {
61 Matrix4x4 bspline_mat( -1, 3, -3, 1,
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);
70 return dot_product(params, cpvec.transformed(bspline_mat) / 6.0);
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,
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);
84 return dot_product(params, cpvec.transformed(crspline_mat) / 2.0);
88 * returns a interpolated scalar value
89 * given 4 control values
91 scalar_t bezier(const Vector4 &cp, scalar_t t)
93 static scalar_t omt, omt3, t3, f;
96 omt3 = omt * omt * omt;
99 return (cp.x * omt3) + (cp.y * f * omt) + (cp.z * f * t) + (cp.w * t3);
103 * Vector3 overloaded bezier function
105 Vector3 bezier(const Vector3 &p0, const Vector3 &p1, const Vector3 &p2, const Vector3 &p3, scalar_t t)
107 static scalar_t omt, omt3, t3, f;
110 omt3 = omt * omt * omt;
113 return (p0 * omt3) + (p1 * f * omt) + (p2 * f * t) + (p3 * t3);
117 * returns a vector tangent to the
118 * Bezier curve at the specified point
120 Vector3 bezier_tangent(const Vector3 &p0, const Vector3 &p1, const Vector3 &p2, const Vector3 &p3, scalar_t 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;
129 p7 = p4 * omt + p5 * t;
130 p8 = p5 * omt + p6 * t;
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();
141 Vector2 pt = p1 + (p2 - p1) * t;
142 return (p - pt).length();
147 i = Vector3(1, 0, 0);
148 j = Vector3(0, 1, 0);
149 k = Vector3(0, 0, 1);
152 Basis::Basis(const Vector3 &i, const Vector3 &j, const Vector3 &k) {
158 Basis::Basis(const Vector3 &dir, bool LeftHanded) {
161 i = cross_product(j, k);
162 j = cross_product(k, i);
166 void Basis::rotate(scalar_t x, scalar_t y, scalar_t z) {
168 RotMat.set_rotation(Vector3(x, y, z));
174 void Basis::rotate(const Vector3 &axis, scalar_t angle) {
176 q.set_rotation(axis, angle);
182 void Basis::rotate(const Matrix4x4 &mat) {
188 void Basis::rotate(const Quaternion &quat) {
194 Matrix3x3 Basis::create_rotation_matrix() const {
195 return Matrix3x3( i.x, j.x, k.x,
203 size_t size_of_scalar_type() {
204 static const scalar_t temp = 0.0;