1 /*****************************************************************************
2 * $LastChangedDate: 2013-05-26 14:26:47 -0400 (Sun, 26 May 2013) $
3 * @file
4 * @author Jim E. Brooks http://www.palomino3d.org
5 * @brief Math functions (vector). See math/funcs.hh.
6 * REMINDERS: - Consider using MATH_EXCEPTION() instead of ASSERT().
7 *//*
8 * LEGAL: COPYRIGHT (C) 2004 JIM E. BROOKS
9 * THIS SOURCE CODE IS RELEASED UNDER THE TERMS
10 * OF THE GNU GENERAL PUBLIC LICENSE VERSION 2 (GPL 2).
11 *****************************************************************************/
12
13 #ifndef MATH_FUNCS_VECTOR_HH
14 #define MATH_FUNCS_VECTOR_HH 1
15
16 #include <utility>
17 #include <cerrno>
18 #include "base/clamp.hh"
19 #include "math/types_trig.hh"
20 #include "math/defs_vector.hh"
21 #include "math/vertex.hh"
22 #include "math/funcs_debug.hh"
23 #include "math/funcs.hh"
24 #include "math/funcs_trig.hh"
25
26 namespace math {
27
28 ////////////////////////////////////////////////////////////////////////////////
29 ////////////////////////////// V E C T O R //////////////////////////////////
30 ////////////////////////////////////////////////////////////////////////////////
31
32 /*****************************************************************************
33 * Distance between vector and implied zero origin.
34 *****************************************************************************/
35 template<typename FP>
36 FP
Distance(const FP x,const FP y,const FP z)37 Distance( const FP x, const FP y, const FP z )
38 {
39 return SquareRoot( (x*x) + (y*y) + (z*z) );
40 }
41
42 INLINE fp
Distance(const Vector2 & v)43 Distance( const Vector2& v )
44 {
45 return SquareRoot( v[XX] * v[XX]
46 + v[YY] * v[YY] );
47 }
48
49 INLINE fp
Distance(const Vector3 & v)50 Distance( const Vector3& v )
51 {
52 return SquareRoot( v[XX] * v[XX]
53 + v[YY] * v[YY]
54 + v[ZZ] * v[ZZ] );
55 }
56
57 /*****************************************************************************
58 * Distance between two vectors (DistanceSquared() is faster than this).
59 *****************************************************************************/
60 template<typename VECTOR3A,typename VECTOR3B>
61 fpx
Distance(const VECTOR3A & v1,const VECTOR3B & v2)62 Distance( const VECTOR3A& v1, const VECTOR3B& v2 )
63 {
64 const fpx dx = v2[XX] - v1[XX]; // vector types may differ,
65 const fpx dy = v2[YY] - v1[YY]; // use extended-precision
66 const fpx dz = v2[ZZ] - v1[ZZ];
67 return SquareRoot( dx*dx + dy*dy + dz*dz );
68 }
69
70 /*****************************************************************************
71 * Calculate the distance^2 (squared) between two vectors.
72 * DISTANCE RETURNED IS SQUARED (distance^2).
73 * SQUARE ROOT OF DISTANCE IS SKIPPED FOR SPEED.
74 * sqrt() and the underlying FPU instruction is very slow.
75 *****************************************************************************/
76 INLINE Vector3::value_type
DistanceSquared(const Vector3 & v1,const Vector3 & v2)77 DistanceSquared( const Vector3& v1, const Vector3& v2 )
78 {
79 Vector3::value_type dx = v2[XX] - v1[XX];
80 Vector3::value_type dy = v2[YY] - v1[YY];
81 Vector3::value_type dz = v2[ZZ] - v1[ZZ];
82 return dx*dx + dy*dy + dz*dz;
83 }
84
85 /*****************************************************************************
86 * @return A normalized vector (unit-length 1.0) of the same vector.
87 * Not to be confused with calculating the normal vector of two vectors (CrossProduct).
88 *****************************************************************************/
89 INLINE Vector3
Normalize(const Vector3 & v)90 Normalize( const Vector3& v )
91 {
92 const Vector3::value_type d = Distance( v );
93 const Vector3::value_type x = v[XX] / d;
94 const Vector3::value_type y = v[YY] / d;
95 const Vector3::value_type z = v[ZZ] / d;
96 MATH_EXCEPTION( (not IfNan(x)) and (not IfNan(y)) and (not IfNan(z)),
97 "math error: Normalize(Vector3) NaN" );
98 return Vector3( x, y, z );
99 }
100
101 template<typename VERTEX,typename FP>
102 VERTEX
103 Normalize( const FP x, const FP y, const FP z )
104 {
105 const typename VERTEX::value_type d = Distance( x, y, z );
106 const FP nx = x / d;
107 const FP ny = y / d;
108 const FP nz = z / d;
109 MATH_EXCEPTION( (not IfNan(nx)) and (not IfNan(ny)) and (not IfNan(nz)),
110 "math error: Normalize(Vector3) NaN" );
111 return VERTEX( nx, ny, nz );
112 }
113
114 /*****************************************************************************
115 * Normalize a 3D point (unit 1 distance) then return one normalized coordinate.
116 * The return value typically is passed to asin().
117 *****************************************************************************/
118 INLINE Vector3::value_type
NormalizeCoord(const uint axis,const Vector3 & v)119 NormalizeCoord( const uint axis, const Vector3& v )
120 {
121 ASSERT_AXIS3(axis);
122 Vector3::value_type d = Distance( v );
123 Vector3::value_type n;
124 if ( axis == XX ) n = v[XX] / d;
125 else if ( axis == YY ) n = v[YY] / d;
126 else /* axis == Z */ n = v[ZZ] / d;
127 MATH_EXCEPTION( not IfNan(n), "math error: NormalizeCoord() NaN" );
128 return n;
129 }
130
131 /*****************************************************************************
132 * Increase/decrease a 3D vector by a scalar.
133 *
134 * The WRONG way, of course, is to simply add the scalar to each dimension.
135 * For example, Vector(1,1) vs. Vector(2,2):
136 * math.sqrt( (1.0*1.0) + (1.0*1.0) )
137 * 1.4142135623730951
138 * math.sqrt( (2.0*2.0) + (2.0*2.0) )
139 * 2.8284271247461903 # oops != 2.4142
140 *
141 * The right way is to turn the scalar distance into a 3D vector
142 * with the same direction as the input vector.
143 * This is done by multiplying the scalar distance
144 * by the normalized input vector, then adding.
145 *****************************************************************************/
146 INLINE Vector3
AddDistance(const Vector3 & v,const fp distance1)147 AddDistance( const Vector3& v, const fp distance1 )
148 {
149 const Vector3 norm3 = Normalize( v );
150 const Vector3 distance3( distance1 * norm3[XX],
151 distance1 * norm3[YY],
152 distance1 * norm3[ZZ] );
153 return Vector3( v[XX] + distance3[XX],
154 v[YY] + distance3[YY],
155 v[ZZ] + distance3[ZZ] );
156 }
157
158 /*****************************************************************************
159 * Calculate a cross product of two vectors which yields a normal vector
160 * that is perpendicular to the plane containing the two source vectors.
161 * Note: The cross product won't be unit length.
162 *****************************************************************************/
163 INLINE Vector3
CrossProduct(const Vector3 & v1,const Vector3 & v2,const Vector3 & v0=Vector3 (0,0,0))164 CrossProduct( const Vector3& v1,
165 const Vector3& v2,
166 const Vector3& v0 = Vector3(0,0,0) ) // origin of v1 and v2
167 {
168 Vector3::value_type xd1 = v1[XX] - v0[XX];
169 Vector3::value_type yd1 = v1[YY] - v0[YY];
170 Vector3::value_type zd1 = v1[ZZ] - v0[ZZ];
171 Vector3::value_type xd2 = v2[XX] - v0[XX];
172 Vector3::value_type yd2 = v2[YY] - v0[YY];
173 Vector3::value_type zd2 = v2[ZZ] - v0[ZZ];
174 return Vector3( yd1 * zd2 - yd2 * zd1, // x: y*z
175 zd1 * xd2 - zd2 * xd1, // y: z*x
176 xd1 * yd2 - xd2 * yd1 ); // z: x*y
177 }
178
179 /*****************************************************************************
180 * Calculate the dot product of two vectors which yields a scalar.
181 * If the angle between the two vectors is between 0 and 90 degrees,
182 * the dot product will be positive, else negative.
183 *****************************************************************************/
184 INLINE fp
DotProduct2(const Vector2 & v1,const Vector2 & v2)185 DotProduct2( const Vector2& v1, const Vector2& v2 )
186 {
187 return v1[XX] * v2[XX]
188 + v1[YY] * v2[YY];
189 }
190
191 template<typename VECTOR3A,typename VECTOR3B>
192 fp
DotProduct3(const VECTOR3A & v1,const VECTOR3B & v2)193 DotProduct3( const VECTOR3A& v1, const VECTOR3B& v2 )
194 {
195 return v1[XX] * v2[XX]
196 + v1[YY] * v2[YY]
197 + v1[ZZ] * v2[ZZ];
198 }
199
200 // Extended-precision.
201 template<typename VECTOR3A,typename VECTOR3B>
202 fpx
DotProduct3_fpx(const VECTOR3A & v1,const VECTOR3B & v2)203 DotProduct3_fpx( const VECTOR3A& v1, const VECTOR3B& v2 )
204 {
205 return fpx(v1[XX]) * fpx(v2[XX])
206 + fpx(v1[YY]) * fpx(v2[YY])
207 + fpx(v1[ZZ]) * fpx(v2[ZZ]);
208 }
209
210 /*****************************************************************************
211 * Compute angle (in radians) between two vectors sharing the same origin.
212 * Unlike a vector, this computed angle has no orientation nor winding direction.
213 * The angle should be always positive and never greater than 180 degrees.
214 * The angle will be the same if one of the vectors is turned in the opposite direction.
215 *****************************************************************************/
216 INLINE Radian
Angle2(const Vector2 & u,const Vector2 & v)217 Angle2( const Vector2& u, const Vector2& v ) // Angle2/3() differ in DotProduct2/3()
218 {
219 // Dot product formula indirectly provides the angle.
220 // angle = arccos( DotProduct(u,v) / Distance(u) * Distance(v) )
221 Vector2::value_type uv = Distance(u) * Distance(v);
222 if ( ABS(uv) < 0.1f ) return 0.0f; // prevent NaN and div by 0
223 Vector2::value_type theta = DotProduct2(u,v) / uv;
224 theta = Clamp( theta, -1.0f, 1.0f ); // acos() won't tolerate 1.00000001
225 errno = 0;
226 Vector2::value_type rad = std::acos( theta );
227 MATH_EXCEPTION( errno == 0, "math error: Angle2() acos()" ); // if acos() failed
228 return rad;
229 }
230
231 template<typename VECTOR>
232 Radian
Angle3(const VECTOR & u,const VECTOR & v)233 Angle3( const VECTOR& u, const VECTOR& v )
234 {
235 // Dot product formula indirectly provides the angle.
236 // angle = arccos( DotProduct(u,v) / Distance(u) * Distance(v) )
237 typename VECTOR::value_type uv = Distance(u) * Distance(v);
238 if ( ABS(uv) < 0.1f ) return 0.0f; // prevent NaN and div by 0
239 typename VECTOR::value_type theta = DotProduct3(u,v) / uv;
240 theta = Clamp( theta, -1.0f, 1.0f ); // acos() won't tolerate 1.00000001
241 errno = 0;
242 typename VECTOR::value_type rad = std::acos( theta );
243 MATH_EXCEPTION( errno == 0, "math error: Angle3() acos()" ); // if acos() failed
244 return rad;
245 }
246
247 /*****************************************************************************
248 * Interpolate between two vectors.
249 * @param v1, v2
250 * @param fraction
251 * Fraction in range {0,..,1.0}.
252 * If fraction is 0.0, v1 is returned.
253 * If fraction is 1.0, v2 is returned.
254 * If fraction is 0.5, midpoint is returned.
255 * @return Interpolated 3D point.
256 *****************************************************************************/
257 INLINE Vector3
Interpolate(const Vector3 & v1,const Vector3 & v2,const fp fraction)258 Interpolate( const Vector3& v1, const Vector3& v2, const fp fraction )
259 {
260 MATH_EXCEPTION( FP_GE(fraction, 0.0f) and FP_LE(fraction, 1.0f), "math error: Interpolate()" );
261 return Vector3( v1 + ((v2-v1) * fraction) );
262 }
263
264 /*****************************************************************************
265 * Midpoint.
266 *****************************************************************************/
267 template<typename T>
268 T
Midpoint(const T & x1,const T & x2)269 Midpoint( const T& x1, const T& x2 ) // could be a vector type
270 {
271 // return x1 + (x2 - x1) / 2;
272 return (x1 + x2) / 2; // faster
273 }
274
275 INLINE Vector3
Midpoint(const Vector3 & v1,const Vector3 & v2)276 Midpoint( const Vector3& v1, const Vector3& v2 )
277 {
278 return Vector3( Midpoint(v1[XX], v2[XX]),
279 Midpoint(v1[YY], v2[YY]),
280 Midpoint(v1[ZZ], v2[ZZ]) );
281 }
282
283 /*****************************************************************************
284 * Cycle {X,Y,Z}.
285 *****************************************************************************/
286 INLINE uint
CycleAxis3(const uint axis)287 CycleAxis3( const uint axis )
288 {
289 return (axis + 1) % 3;
290 }
291
292 /*****************************************************************************
293 * Rotate a 3D vector around an axis.
294 *****************************************************************************/
295 template<typename VECTOR>
296 VECTOR
RotateVector3(const VECTOR & v,const uint axis,const Radian rad)297 RotateVector3( const VECTOR& v, const uint axis, const Radian rad )
298 {
299 // If angle is 0 then sin(0),cos(0) caused multiplying by 0,1, resp.
300 // The rotation pattern is:
301 // c - s
302 // s + c
303 const std::pair<fp,fp> s_c = SinCos<fp>( rad );
304 const fp& s = s_c.first;
305 const fp& c = s_c.second;
306 switch ( axis )
307 {
308 case XX:
309 {
310 return VECTOR( v[XX],
311 v[YY] * c - v[ZZ] * s,
312 v[YY] * s + v[ZZ] * c );
313 }
314 break;
315
316 case YY:
317 {
318 return VECTOR( v[XX] * c - v[ZZ] * s,
319 v[YY],
320 v[XX] * s + v[ZZ] * c );
321 }
322 break;
323
324 case ZZ:
325 {
326 return VECTOR( v[XX] * c - v[YY] * s,
327 v[XX] * s + v[YY] * c,
328 v[ZZ] );
329 }
330 break;
331
332 default: ASSERT(false); return VECTOR(); break;
333 }
334 }
335
336 } // namespace math
337
338 #endif // MATH_FUNCS_VECTOR_HH
339