Dogcows Code - chaz/yoink/blob - src/Moof/cml/mathlib/vector_misc.h

   1 /* -*- C++ -*- ------------------------------------------------------------
   2
   3 Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
   4
   5 The Configurable Math Library (CML) is distributed under the terms of the
   6 Boost Software License, v1.0 (see cml/LICENSE for details).
   7
   8  *-----------------------------------------------------------------------*/
   9 /** @file
  10  *  @brief
  11  */
  12
  13 #ifndef vector_misc_h
  14 #define vector_misc_h
  15
  16 #include <cml/mathlib/coord_conversion.h>
  17
  18 /* Miscellaneous vector functions. */
  19
  20 namespace cml {
  21
  22 /* Function to project a vector v onto a hyperplane specified by a unit-length
  23  * normal n.
  24  *
  25  * @todo: Clean up promotion code.
  26  */
  27
  28 template < class VecT_1, class VecT_2 >
  29 typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector
  30 project_to_hplane(const VecT_1& v, const VecT_2& n)
  31 {
  32     typedef typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector
  33         result_type;
  34
  35     result_type result;
  36     et::detail::Resize(result, v.size());
  37
  38     result = v - dot(v,n) * n;
  39     return result;
  40 }
  41
  42 /* Return a vector perpendicular (CCW) to a 2D vector. */
  43 template < class VecT > vector< typename VecT::value_type, fixed<2> >
  44 perp(const VecT& v)
  45 {
  46     typedef vector< typename VecT::value_type, fixed<2> > temporary_type;
  47
  48     /* Checking */
  49     detail::CheckVec2(v);
  50
  51     return temporary_type(-v[1],v[0]);
  52 }
  53
  54 /* @todo: unit_cross() and cross_cardinal() should probably go in
  55  * vector_products.h, but I'm trying to avoid modifying the existing codebase
  56  * for now.
  57  */
  58
  59 /** Return normalized cross product of two vectors */
  60 template< class LeftT, class RightT >
  61 typename detail::CrossPromote<LeftT,RightT>::promoted_vector
  62 unit_cross(const LeftT& left, const RightT& right) {
  63     /* @todo: This will probably break with dynamic<> vectors */
  64     return normalize(cross(left,right));
  65 }
  66
  67 /** Return the cross product of v and the i'th cardinal basis vector */
  68 template < class VecT > vector< typename VecT::value_type, fixed<3> >
  69 cross_cardinal(const VecT& v, size_t i)
  70 {
  71     typedef vector< typename VecT::value_type, fixed<3> > vector_type;
  72     typedef typename vector_type::value_type value_type;
  73
  74     /* Checking */
  75     detail::CheckVec3(v);
  76     detail::CheckIndex3(i);
  77
  78     size_t j, k;
  79     cyclic_permutation(i, i, j, k);
  80     vector_type result;
  81     result[i] = value_type(0);
  82     result[j] = v[k];
  83     result[k] = -v[j];
  84     return result;
  85 }
  86
  87 /** Return the cross product of the i'th cardinal basis vector and v */
  88 template < class VecT > vector< typename VecT::value_type, fixed<3> >
  89 cross_cardinal(size_t i, const VecT& v)
  90 {
  91     typedef vector< typename VecT::value_type, fixed<3> > vector_type;
  92     typedef typename vector_type::value_type value_type;
  93
  94     /* Checking */
  95     detail::CheckVec3(v);
  96     detail::CheckIndex3(i);
  97
  98     size_t j, k;
  99     cyclic_permutation(i, i, j, k);
 100     vector_type result;
 101     result[i] = value_type(0);
 102     result[j] = -v[k];
 103     result[k] = v[j];
 104     return result;
 105 }
 106
 107 /** Rotate a 3D vector v about a unit-length vector n. */
 108 template< class VecT_1, class VecT_2, typename Real >
 109 vector<
 110     typename et::ScalarPromote<
 111         typename VecT_1::value_type,
 112         typename VecT_2::value_type
 113     >::type,
 114     fixed<3>
 115 >
 116 rotate_vector(const VecT_1& v, const VecT_2& n, Real angle)
 117 {
 118     typedef vector<
 119         typename et::ScalarPromote<
 120             typename VecT_1::value_type,
 121             typename VecT_2::value_type
 122         >::type,
 123         fixed<3>
 124     > result_type;
 125
 126     /* Checking */
 127     detail::CheckVec3(v);
 128     detail::CheckVec3(n);
 129
 130     result_type parallel = dot(v,n)*n;
 131     return (
 132         std::cos(angle)*(v-parallel) + std::sin(angle)*cross(n,v) + parallel
 133     );
 134 }
 135
 136 /** Rotate a 2D vector v about a unit-length vector n. */
 137 template< class VecT, typename Real >
 138 vector< typename VecT::value_type, fixed<2> >
 139 rotate_vector_2D(const VecT& v, Real angle)
 140 {
 141     typedef vector< typename VecT::value_type, fixed<2> > result_type;
 142     typedef typename result_type::value_type value_type;
 143
 144     /* Checking */
 145     detail::CheckVec2(v);
 146
 147     value_type s = std::sin(angle);
 148     value_type c = std::cos(angle);
 149
 150     return result_type(c * v[0] - s * v[1], s * v[0] + c * v[1]);
 151 }
 152
 153 /** Random unit 3D or 2D vector
 154  *
 155  * @todo: This is just placeholder code for what will be a more thorough
 156  * 'random unit' implementation:
 157  *
 158  * - All dimensions will be handled uniformly if practical, perhaps through
 159  *   a normal distrubution PRNG.
 160  *
 161  * - Failing that (or perhaps even in this case), dimensions 2 and 3 will be
 162  *   dispatched to special-case code, most likely implementing the algorithms
 163  *   below.
 164  *
 165  * - Like the utility random functions, the option of using one's own PRGN
 166  *   will be made available.
 167  *
 168  * @todo: Once N-d random vectors are supported, add a 'random unit
 169  * quaternion' function that wraps a call to random_unit() with a 4D vector as
 170  * the argument.
 171  */
 172 template < typename E, class A > void
 173 random_unit(vector<E,A>& v)
 174 {
 175     typedef vector<E,A> vector_type;
 176     typedef typename vector_type::value_type value_type;
 177
 178     switch (v.size()) {
 179         case 3:
 180         {
 181             vector< E, fixed<3> > temp;
 182             spherical_to_cartesian(
 183                 value_type(1),
 184                 value_type(random_unit() * constants<value_type>::two_pi()),
 185                 acos_safe(random_real(value_type(-1),value_type(1))),
 186                 2,
 187                 colatitude,
 188                 temp
 189             );
 190             v[0] = temp[0];
 191             v[1] = temp[1];
 192             v[2] = temp[2];
 193             break;
 194         }
 195         case 2:
 196         {
 197             vector< E, fixed<2> > temp;
 198             polar_to_cartesian(
 199                 value_type(1),
 200                 value_type(random_unit() * constants<value_type>::two_pi()),
 201                 temp
 202             );
 203             v[0] = temp[0];
 204             v[1] = temp[1];
 205             break;
 206         }
 207         default:
 208             throw std::invalid_argument(
 209                 "random_unit() for N-d vectors not implemented yet");
 210             break;
 211     }
 212 }
 213
 214 /* Random vector within a given angle of a unit-length axis, i.e. in a cone
 215  * (3D) or wedge (2D).
 216  *
 217  * The same notes the appear above apply here too, more or less. One
 218  * difference is that this is really only useful in 2D and 3D (presumably), so
 219  * we'll probably just do a compile- or run-time dispatch as appropriate.
 220  *
 221  * Also, there may be a better algorithm for generating a random unit vector
 222  * in a cone; need to look into that.
 223  *
 224  * All of this 'temp' stuff is because there's no compile-time dispatch for
 225  * 3D and 2D vectors, but that'll be fixed soon.
 226  */
 227
 228 template < typename E, class A, class VecT > void
 229 random_unit(vector<E,A>& v, const VecT& axis, E theta)
 230 {
 231     typedef vector<E,A> vector_type;
 232     typedef typename vector_type::value_type value_type;
 233
 234     switch (v.size()) {
 235         case 3:
 236         {
 237             vector< E, fixed<3> > temp, n, temp_axis;
 238             temp_axis[0] = axis[0];
 239             temp_axis[1] = axis[1];
 240             temp_axis[2] = axis[2];
 241
 242             /* @todo: Function for finding 'any perpendicular vector'? */
 243             n = axis_3D(cml::index_of_min_abs(axis[0],axis[1],axis[2]));
 244             n = cross(n,temp_axis);
 245
 246             /* Rotate v 'away from' the axis by a random angle in the range
 247              * [-theta,theta]
 248              */
 249             temp = rotate_vector(temp_axis,n,random_real(-theta,theta));
 250
 251             /* Rotate v about the axis by a random angle in the range [-pi,pi]
 252              */
 253             temp = rotate_vector(
 254                 temp,
 255                 temp_axis,
 256                 random_real(
 257                     -constants<value_type>::pi(),
 258                      constants<value_type>::pi()
 259                 )
 260             );
 261
 262             v[0] = temp[0];
 263             v[1] = temp[1];
 264             v[2] = temp[2];
 265             break;
 266         }
 267         case 2:
 268         {
 269             vector< E, fixed<2> > temp, temp_axis;
 270             temp_axis[0] = axis[0];
 271             temp_axis[1] = axis[1];
 272             temp = rotate_vector_2D(temp_axis, random_real(-theta,theta));
 273             v[0] = temp[0];
 274             v[1] = temp[1];
 275             break;
 276         }
 277         default:
 278             throw std::invalid_argument(
 279                 "random_unit(v,axis,theta) only implemented for 2D and 3D");
 280             break;
 281     }
 282 }
 283
 284 /* NEW: Manhattan distance */
 285
 286 template< class VecT_1, class VecT_2 >
 287 typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
 288 manhattan_distance(const VecT_1& v1, const VecT_2& v2) {
 289     /* Check that a promotion exists */
 290     typedef typename et::VectorPromote<
 291         VecT_1,VecT_2>::temporary_type promoted_vector;
 292
 293     typedef typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar scalar_type;
 294
 295     scalar_type sum = scalar_type(0);
 296     for (size_t i = 0; i < v1.size(); ++i) {
 297         sum += std::fabs(v2[i]-v1[i]);
 298     }
 299     return sum;
 300 }
 301
 302 } // namespace cml
 303
 304 #endif