dali-physics/third-party/chipmunk2d/include/chipmunk/cpVect.h

   1 /* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
   2  *
   3  * Permission is hereby granted, free of charge, to any person obtaining a copy
   4  * of this software and associated documentation files (the "Software"), to deal
   5  * in the Software without restriction, including without limitation the rights
   6  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   7  * copies of the Software, and to permit persons to whom the Software is
   8  * furnished to do so, subject to the following conditions:
   9  *
  10  * The above copyright notice and this permission notice shall be included in
  11  * all copies or substantial portions of the Software.
  12  *
  13  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  14  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  15  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  16  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  17  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  18  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  19  * SOFTWARE.
  20  */
  21
  22 #ifndef CHIPMUNK_VECT_H
  23 #define CHIPMUNK_VECT_H
  24
  25 #include "chipmunk_types.h"
  26
  27 /// @defgroup cpVect cpVect
  28 /// Chipmunk's 2D vector type along with a handy 2D vector math lib.
  29 /// @{
  30
  31 /// Constant for the zero vector.
  32 static const cpVect cpvzero = {0.0f,0.0f};
  33
  34 /// Convenience constructor for cpVect structs.
  35 static inline cpVect cpv(const cpFloat x, const cpFloat y)
  36 {
  37         cpVect v = {x, y};
  38         return v;
  39 }
  40
  41 /// Check if two vectors are equal. (Be careful when comparing floating point numbers!)
  42 static inline cpBool cpveql(const cpVect v1, const cpVect v2)
  43 {
  44         return (v1.x == v2.x && v1.y == v2.y);
  45 }
  46
  47 /// Add two vectors
  48 static inline cpVect cpvadd(const cpVect v1, const cpVect v2)
  49 {
  50         return cpv(v1.x + v2.x, v1.y + v2.y);
  51 }
  52
  53 /// Subtract two vectors.
  54 static inline cpVect cpvsub(const cpVect v1, const cpVect v2)
  55 {
  56         return cpv(v1.x - v2.x, v1.y - v2.y);
  57 }
  58
  59 /// Negate a vector.
  60 static inline cpVect cpvneg(const cpVect v)
  61 {
  62         return cpv(-v.x, -v.y);
  63 }
  64
  65 /// Scalar multiplication.
  66 static inline cpVect cpvmult(const cpVect v, const cpFloat s)
  67 {
  68         return cpv(v.x*s, v.y*s);
  69 }
  70
  71 /// Vector dot product.
  72 static inline cpFloat cpvdot(const cpVect v1, const cpVect v2)
  73 {
  74         return v1.x*v2.x + v1.y*v2.y;
  75 }
  76
  77 /// 2D vector cross product analog.
  78 /// The cross product of 2D vectors results in a 3D vector with only a z component.
  79 /// This function returns the magnitude of the z value.
  80 static inline cpFloat cpvcross(const cpVect v1, const cpVect v2)
  81 {
  82         return v1.x*v2.y - v1.y*v2.x;
  83 }
  84
  85 /// Returns a perpendicular vector. (90 degree rotation)
  86 static inline cpVect cpvperp(const cpVect v)
  87 {
  88         return cpv(-v.y, v.x);
  89 }
  90
  91 /// Returns a perpendicular vector. (-90 degree rotation)
  92 static inline cpVect cpvrperp(const cpVect v)
  93 {
  94         return cpv(v.y, -v.x);
  95 }
  96
  97 /// Returns the vector projection of v1 onto v2.
  98 static inline cpVect cpvproject(const cpVect v1, const cpVect v2)
  99 {
 100         return cpvmult(v2, cpvdot(v1, v2)/cpvdot(v2, v2));
 101 }
 102
 103 /// Returns the unit length vector for the given angle (in radians).
 104 static inline cpVect cpvforangle(const cpFloat a)
 105 {
 106         return cpv(cpfcos(a), cpfsin(a));
 107 }
 108
 109 /// Returns the angular direction v is pointing in (in radians).
 110 static inline cpFloat cpvtoangle(const cpVect v)
 111 {
 112         return cpfatan2(v.y, v.x);
 113 }
 114
 115 /// Uses complex number multiplication to rotate v1 by v2. Scaling will occur if v1 is not a unit vector.
 116 static inline cpVect cpvrotate(const cpVect v1, const cpVect v2)
 117 {
 118         return cpv(v1.x*v2.x - v1.y*v2.y, v1.x*v2.y + v1.y*v2.x);
 119 }
 120
 121 /// Inverse of cpvrotate().
 122 static inline cpVect cpvunrotate(const cpVect v1, const cpVect v2)
 123 {
 124         return cpv(v1.x*v2.x + v1.y*v2.y, v1.y*v2.x - v1.x*v2.y);
 125 }
 126
 127 /// Returns the squared length of v. Faster than cpvlength() when you only need to compare lengths.
 128 static inline cpFloat cpvlengthsq(const cpVect v)
 129 {
 130         return cpvdot(v, v);
 131 }
 132
 133 /// Returns the length of v.
 134 static inline cpFloat cpvlength(const cpVect v)
 135 {
 136         return cpfsqrt(cpvdot(v, v));
 137 }
 138
 139 /// Linearly interpolate between v1 and v2.
 140 static inline cpVect cpvlerp(const cpVect v1, const cpVect v2, const cpFloat t)
 141 {
 142         return cpvadd(cpvmult(v1, 1.0f - t), cpvmult(v2, t));
 143 }
 144
 145 /// Returns a normalized copy of v.
 146 static inline cpVect cpvnormalize(const cpVect v)
 147 {
 148         // Neat trick I saw somewhere to avoid div/0.
 149         return cpvmult(v, 1.0f/(cpvlength(v) + CPFLOAT_MIN));
 150 }
 151
 152 /// Spherical linearly interpolate between v1 and v2.
 153 static inline cpVect
 154 cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t)
 155 {
 156         cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2));
 157         cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f));
 158
 159         if(omega < 1e-3){
 160                 // If the angle between two vectors is very small, lerp instead to avoid precision issues.
 161                 return cpvlerp(v1, v2, t);
 162         } else {
 163                 cpFloat denom = 1.0f/cpfsin(omega);
 164                 return cpvadd(cpvmult(v1, cpfsin((1.0f - t)*omega)*denom), cpvmult(v2, cpfsin(t*omega)*denom));
 165         }
 166 }
 167
 168 /// Spherical linearly interpolate between v1 towards v2 by no more than angle a radians
 169 static inline cpVect
 170 cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a)
 171 {
 172         cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2));
 173         cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f));
 174
 175         return cpvslerp(v1, v2, cpfmin(a, omega)/omega);
 176 }
 177
 178 /// Clamp v to length len.
 179 static inline cpVect cpvclamp(const cpVect v, const cpFloat len)
 180 {
 181         return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v;
 182 }
 183
 184 /// Linearly interpolate between v1 towards v2 by distance d.
 185 static inline cpVect cpvlerpconst(cpVect v1, cpVect v2, cpFloat d)
 186 {
 187         return cpvadd(v1, cpvclamp(cpvsub(v2, v1), d));
 188 }
 189
 190 /// Returns the distance between v1 and v2.
 191 static inline cpFloat cpvdist(const cpVect v1, const cpVect v2)
 192 {
 193         return cpvlength(cpvsub(v1, v2));
 194 }
 195
 196 /// Returns the squared distance between v1 and v2. Faster than cpvdist() when you only need to compare distances.
 197 static inline cpFloat cpvdistsq(const cpVect v1, const cpVect v2)
 198 {
 199         return cpvlengthsq(cpvsub(v1, v2));
 200 }
 201
 202 /// Returns true if the distance between v1 and v2 is less than dist.
 203 static inline cpBool cpvnear(const cpVect v1, const cpVect v2, const cpFloat dist)
 204 {
 205         return cpvdistsq(v1, v2) < dist*dist;
 206 }
 207
 208 /// @}
 209
 210 /// @defgroup cpMat2x2 cpMat2x2
 211 /// 2x2 matrix type used for tensors and such.
 212 /// @{
 213
 214 // NUKE
 215 static inline cpMat2x2
 216 cpMat2x2New(cpFloat a, cpFloat b, cpFloat c, cpFloat d)
 217 {
 218         cpMat2x2 m = {a, b, c, d};
 219         return m;
 220 }
 221
 222 static inline cpVect
 223 cpMat2x2Transform(cpMat2x2 m, cpVect v)
 224 {
 225         return cpv(v.x*m.a + v.y*m.b, v.x*m.c + v.y*m.d);
 226 }
 227
 228 ///@}
 229
 230 #endif