Bart Janssens / SVL

Simple Vector Library 1.5 http://www.cs.cmu.edu/~ajw/doc/svl.html

Quat.h@1:e25ff4b06ed2, 2016-01-05 (annotated)

Committer:
BartJanssens
Date:
Tue Jan 05 13:37:50 2016 +0000
Revision:
1:e25ff4b06ed2
Parent:
0:785cff1e5a7c 
fixed va_list bug

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BartJanssens 0:785cff1e5a7c 1 #ifndef __Quat__
BartJanssens 0:785cff1e5a7c 2 #define __Quat__
BartJanssens 0:785cff1e5a7c 3 #include "Vec3.h"
BartJanssens 0:785cff1e5a7c 4 #include "Mat3.h"
BartJanssens 0:785cff1e5a7c 5 #include "Vec4.h"
BartJanssens 0:785cff1e5a7c 6 #include "Mat4.h"
BartJanssens 0:785cff1e5a7c 7
BartJanssens 0:785cff1e5a7c 8 class Quat
BartJanssens 0:785cff1e5a7c 9 {
BartJanssens 0:785cff1e5a7c 10 public:
BartJanssens 0:785cff1e5a7c 11 // constructors
BartJanssens 0:785cff1e5a7c 12 Quat();
BartJanssens 0:785cff1e5a7c 13 Quat(Real q0, Real q1, Real q2, Real q3); // [q0,(q1,q2,q3)]
BartJanssens 0:785cff1e5a7c 14 Quat (const Vec3 &axis, Real angle);
BartJanssens 0:785cff1e5a7c 15 Quat (const Mat3 &m);
BartJanssens 0:785cff1e5a7c 16
BartJanssens 0:785cff1e5a7c 17 Int Elts() const { return (4); };
BartJanssens 0:785cff1e5a7c 18
BartJanssens 0:785cff1e5a7c 19 Real &operator [] (Int i);
BartJanssens 0:785cff1e5a7c 20 const Real &operator [] (Int i) const;
BartJanssens 0:785cff1e5a7c 21
BartJanssens 0:785cff1e5a7c 22 // Assignment operators
BartJanssens 0:785cff1e5a7c 23
BartJanssens 0:785cff1e5a7c 24 Quat &operator = (const Quat &a);
BartJanssens 0:785cff1e5a7c 25 Quat &operator += (const Quat &a);
BartJanssens 0:785cff1e5a7c 26 Quat &operator -= (const Quat &a);
BartJanssens 0:785cff1e5a7c 27 Quat &operator *= (const Quat &a);
BartJanssens 0:785cff1e5a7c 28 Quat &operator *= (Real s);
BartJanssens 0:785cff1e5a7c 29 Quat &operator /= (Real s);
BartJanssens 0:785cff1e5a7c 30
BartJanssens 0:785cff1e5a7c 31 // Arithmetic operators
BartJanssens 0:785cff1e5a7c 32
BartJanssens 0:785cff1e5a7c 33 Quat operator + (const Quat &a) const; // v + a
BartJanssens 0:785cff1e5a7c 34 Quat operator - (const Quat &a) const; // v - a
BartJanssens 0:785cff1e5a7c 35 Quat operator - () const; // -v
BartJanssens 0:785cff1e5a7c 36 Quat operator * (const Quat &a) const; // v * a (vx * ax, ...)
BartJanssens 0:785cff1e5a7c 37 Quat operator * (Real s) const; // v * s
BartJanssens 0:785cff1e5a7c 38 Quat operator / (Real s) const; // v / s
BartJanssens 0:785cff1e5a7c 39
BartJanssens 0:785cff1e5a7c 40
BartJanssens 0:785cff1e5a7c 41 Quat &Normalise(); // normalise vector
BartJanssens 0:785cff1e5a7c 42
BartJanssens 0:785cff1e5a7c 43 protected:
BartJanssens 0:785cff1e5a7c 44 Real elt[4];
BartJanssens 0:785cff1e5a7c 45 };
BartJanssens 0:785cff1e5a7c 46
BartJanssens 0:785cff1e5a7c 47 inline Quat operator * (Real s, const Quat &v); // Left mult. by s
BartJanssens 0:785cff1e5a7c 48 inline Real dot(const Quat &a, const Quat &b); // v . a
BartJanssens 0:785cff1e5a7c 49 inline Real len(const Quat &v); // || v ||
BartJanssens 0:785cff1e5a7c 50 inline Real sqrlen(const Quat &v); // v . v
BartJanssens 0:785cff1e5a7c 51 inline Quat norm(const Quat &v); // v / || v ||
BartJanssens 0:785cff1e5a7c 52 inline Void normalise(Quat &v); // v = norm(v)
BartJanssens 0:785cff1e5a7c 53 inline Quat slerp(const Quat &q1, const Quat &q2, Real t);
BartJanssens 0:785cff1e5a7c 54 inline Quat conjugate(const Quat &q);
BartJanssens 0:785cff1e5a7c 55
BartJanssens 0:785cff1e5a7c 56 Mat3 Rot3(const Quat &q);
BartJanssens 0:785cff1e5a7c 57 Mat4 HRot4(const Quat &q);
BartJanssens 0:785cff1e5a7c 58
BartJanssens 0:785cff1e5a7c 59
BartJanssens 0:785cff1e5a7c 60 //std::ostream &operator << (std::ostream &s, const Quat &v);
BartJanssens 0:785cff1e5a7c 61 //std::istream &operator >> (std::istream &s, Quat &v);
BartJanssens 0:785cff1e5a7c 62
BartJanssens 0:785cff1e5a7c 63 void printQuat(const Quat &v);
BartJanssens 0:785cff1e5a7c 64
BartJanssens 0:785cff1e5a7c 65 inline Real &Quat::operator [] (Int i)
BartJanssens 0:785cff1e5a7c 66 {
BartJanssens 0:785cff1e5a7c 67 CheckRange(i, 0, 4, "(Quat::[i]) index out of range");
BartJanssens 0:785cff1e5a7c 68 return(elt[i]);
BartJanssens 0:785cff1e5a7c 69 }
BartJanssens 0:785cff1e5a7c 70
BartJanssens 0:785cff1e5a7c 71 inline const Real &Quat::operator [] (Int i) const
BartJanssens 0:785cff1e5a7c 72 {
BartJanssens 0:785cff1e5a7c 73 CheckRange(i, 0, 4, "(Quat::[i]) index out of range");
BartJanssens 0:785cff1e5a7c 74 return(elt[i]);
BartJanssens 0:785cff1e5a7c 75 }
BartJanssens 0:785cff1e5a7c 76
BartJanssens 0:785cff1e5a7c 77 inline Quat::Quat()
BartJanssens 0:785cff1e5a7c 78 {
BartJanssens 0:785cff1e5a7c 79 }
BartJanssens 0:785cff1e5a7c 80
BartJanssens 0:785cff1e5a7c 81 inline Quat::Quat(Real q0, Real q1, Real q2, Real q3)
BartJanssens 0:785cff1e5a7c 82 {
BartJanssens 0:785cff1e5a7c 83 elt[0] = q0;
BartJanssens 0:785cff1e5a7c 84 elt[1] = q1;
BartJanssens 0:785cff1e5a7c 85 elt[2] = q2;
BartJanssens 0:785cff1e5a7c 86 elt[3] = q3;
BartJanssens 0:785cff1e5a7c 87 }
BartJanssens 0:785cff1e5a7c 88
BartJanssens 0:785cff1e5a7c 89 inline Quat::Quat(const Vec3 &axis, Real angle)
BartJanssens 0:785cff1e5a7c 90 {
BartJanssens 0:785cff1e5a7c 91 Vec3 n = norm(axis);
BartJanssens 0:785cff1e5a7c 92 Real sinhalf = sin(angle/2);
BartJanssens 0:785cff1e5a7c 93 elt[1] = sinhalf*n[0];
BartJanssens 0:785cff1e5a7c 94 elt[2] = sinhalf*n[1];
BartJanssens 0:785cff1e5a7c 95 elt[3] = sinhalf*n[2];
BartJanssens 0:785cff1e5a7c 96
BartJanssens 0:785cff1e5a7c 97 elt[0] = cos(angle/2);
BartJanssens 0:785cff1e5a7c 98 }
BartJanssens 0:785cff1e5a7c 99
BartJanssens 0:785cff1e5a7c 100
BartJanssens 0:785cff1e5a7c 101 inline Quat &Quat::operator = (const Quat &v)
BartJanssens 0:785cff1e5a7c 102 {
BartJanssens 0:785cff1e5a7c 103 elt[0] = v[0];
BartJanssens 0:785cff1e5a7c 104 elt[1] = v[1];
BartJanssens 0:785cff1e5a7c 105 elt[2] = v[2];
BartJanssens 0:785cff1e5a7c 106 elt[3] = v[3];
BartJanssens 0:785cff1e5a7c 107
BartJanssens 0:785cff1e5a7c 108 return(SELF);
BartJanssens 0:785cff1e5a7c 109 }
BartJanssens 0:785cff1e5a7c 110
BartJanssens 0:785cff1e5a7c 111 inline Quat &Quat::operator += (const Quat &v)
BartJanssens 0:785cff1e5a7c 112 {
BartJanssens 0:785cff1e5a7c 113 elt[0] += v[0];
BartJanssens 0:785cff1e5a7c 114 elt[1] += v[1];
BartJanssens 0:785cff1e5a7c 115 elt[2] += v[2];
BartJanssens 0:785cff1e5a7c 116 elt[3] += v[3];
BartJanssens 0:785cff1e5a7c 117
BartJanssens 0:785cff1e5a7c 118 return(SELF);
BartJanssens 0:785cff1e5a7c 119 }
BartJanssens 0:785cff1e5a7c 120
BartJanssens 0:785cff1e5a7c 121 inline Quat &Quat::operator -= (const Quat &v)
BartJanssens 0:785cff1e5a7c 122 {
BartJanssens 0:785cff1e5a7c 123 elt[0] -= v[0];
BartJanssens 0:785cff1e5a7c 124 elt[1] -= v[1];
BartJanssens 0:785cff1e5a7c 125 elt[2] -= v[2];
BartJanssens 0:785cff1e5a7c 126 elt[3] -= v[3];
BartJanssens 0:785cff1e5a7c 127
BartJanssens 0:785cff1e5a7c 128 return(SELF);
BartJanssens 0:785cff1e5a7c 129 }
BartJanssens 0:785cff1e5a7c 130
BartJanssens 0:785cff1e5a7c 131 inline Quat &Quat::operator *= (const Quat &v)
BartJanssens 0:785cff1e5a7c 132 {
BartJanssens 0:785cff1e5a7c 133 Quat tmp(elt[0],elt[1],elt[2],elt[3]);
BartJanssens 0:785cff1e5a7c 134 tmp = tmp * v;
BartJanssens 0:785cff1e5a7c 135
BartJanssens 0:785cff1e5a7c 136 elt[0] = tmp[0];
BartJanssens 0:785cff1e5a7c 137 elt[1] = tmp[1];
BartJanssens 0:785cff1e5a7c 138 elt[2] = tmp[2];
BartJanssens 0:785cff1e5a7c 139 elt[3] = tmp[3];
BartJanssens 0:785cff1e5a7c 140
BartJanssens 0:785cff1e5a7c 141 return(SELF);
BartJanssens 0:785cff1e5a7c 142 }
BartJanssens 0:785cff1e5a7c 143
BartJanssens 0:785cff1e5a7c 144 inline Quat &Quat::operator *= (Real s)
BartJanssens 0:785cff1e5a7c 145 {
BartJanssens 0:785cff1e5a7c 146 elt[0] *= s;
BartJanssens 0:785cff1e5a7c 147 elt[1] *= s;
BartJanssens 0:785cff1e5a7c 148 elt[2] *= s;
BartJanssens 0:785cff1e5a7c 149 elt[3] *= s;
BartJanssens 0:785cff1e5a7c 150
BartJanssens 0:785cff1e5a7c 151 return(SELF);
BartJanssens 0:785cff1e5a7c 152 }
BartJanssens 0:785cff1e5a7c 153
BartJanssens 0:785cff1e5a7c 154 inline Quat &Quat::operator /= (Real s)
BartJanssens 0:785cff1e5a7c 155 {
BartJanssens 0:785cff1e5a7c 156 elt[0] /= s;
BartJanssens 0:785cff1e5a7c 157 elt[1] /= s;
BartJanssens 0:785cff1e5a7c 158 elt[2] /= s;
BartJanssens 0:785cff1e5a7c 159 elt[3] /= s;
BartJanssens 0:785cff1e5a7c 160
BartJanssens 0:785cff1e5a7c 161 return(SELF);
BartJanssens 0:785cff1e5a7c 162 }
BartJanssens 0:785cff1e5a7c 163
BartJanssens 0:785cff1e5a7c 164
BartJanssens 0:785cff1e5a7c 165 inline Quat Quat::operator + (const Quat &a) const
BartJanssens 0:785cff1e5a7c 166 {
BartJanssens 0:785cff1e5a7c 167 Quat result;
BartJanssens 0:785cff1e5a7c 168
BartJanssens 0:785cff1e5a7c 169 result[0] = elt[0] + a[0];
BartJanssens 0:785cff1e5a7c 170 result[1] = elt[1] + a[1];
BartJanssens 0:785cff1e5a7c 171 result[2] = elt[2] + a[2];
BartJanssens 0:785cff1e5a7c 172 result[3] = elt[3] + a[3];
BartJanssens 0:785cff1e5a7c 173
BartJanssens 0:785cff1e5a7c 174 return(result);
BartJanssens 0:785cff1e5a7c 175 }
BartJanssens 0:785cff1e5a7c 176
BartJanssens 0:785cff1e5a7c 177 inline Quat Quat::operator - (const Quat &a) const
BartJanssens 0:785cff1e5a7c 178 {
BartJanssens 0:785cff1e5a7c 179 Quat result;
BartJanssens 0:785cff1e5a7c 180
BartJanssens 0:785cff1e5a7c 181 result[0] = elt[0] - a[0];
BartJanssens 0:785cff1e5a7c 182 result[1] = elt[1] - a[1];
BartJanssens 0:785cff1e5a7c 183 result[2] = elt[2] - a[2];
BartJanssens 0:785cff1e5a7c 184 result[3] = elt[3] - a[3];
BartJanssens 0:785cff1e5a7c 185
BartJanssens 0:785cff1e5a7c 186 return(result);
BartJanssens 0:785cff1e5a7c 187 }
BartJanssens 0:785cff1e5a7c 188
BartJanssens 0:785cff1e5a7c 189 inline Quat Quat::operator - () const
BartJanssens 0:785cff1e5a7c 190 {
BartJanssens 0:785cff1e5a7c 191 Quat result;
BartJanssens 0:785cff1e5a7c 192
BartJanssens 0:785cff1e5a7c 193 result[0] = -elt[0];
BartJanssens 0:785cff1e5a7c 194 result[1] = -elt[1];
BartJanssens 0:785cff1e5a7c 195 result[2] = -elt[2];
BartJanssens 0:785cff1e5a7c 196 result[3] = -elt[3];
BartJanssens 0:785cff1e5a7c 197
BartJanssens 0:785cff1e5a7c 198 return(result);
BartJanssens 0:785cff1e5a7c 199 }
BartJanssens 0:785cff1e5a7c 200
BartJanssens 0:785cff1e5a7c 201 inline Quat Quat::operator * (const Quat &a) const
BartJanssens 0:785cff1e5a7c 202 {
BartJanssens 0:785cff1e5a7c 203 Quat result;
BartJanssens 0:785cff1e5a7c 204
BartJanssens 0:785cff1e5a7c 205 Vec3 qv(elt[1],elt[2],elt[3]); Real qs = elt[0];
BartJanssens 0:785cff1e5a7c 206 Vec3 av(a[1],a[2],a[3]); Real as = a[0];
BartJanssens 0:785cff1e5a7c 207
BartJanssens 0:785cff1e5a7c 208 Vec3 rv = qs*av + as*qv + cross(qv,av);
BartJanssens 0:785cff1e5a7c 209 Real rs = qs*as - dot(qv,av);
BartJanssens 0:785cff1e5a7c 210
BartJanssens 0:785cff1e5a7c 211 result[1] = rv[0];
BartJanssens 0:785cff1e5a7c 212 result[2] = rv[1];
BartJanssens 0:785cff1e5a7c 213 result[3] = rv[2];
BartJanssens 0:785cff1e5a7c 214 result[0] = rs;
BartJanssens 0:785cff1e5a7c 215
BartJanssens 0:785cff1e5a7c 216 return(result);
BartJanssens 0:785cff1e5a7c 217 }
BartJanssens 0:785cff1e5a7c 218
BartJanssens 0:785cff1e5a7c 219 inline Quat Quat::operator * (Real s) const
BartJanssens 0:785cff1e5a7c 220 {
BartJanssens 0:785cff1e5a7c 221 Quat result;
BartJanssens 0:785cff1e5a7c 222
BartJanssens 0:785cff1e5a7c 223 result[0] = elt[0] * s;
BartJanssens 0:785cff1e5a7c 224 result[1] = elt[1] * s;
BartJanssens 0:785cff1e5a7c 225 result[2] = elt[2] * s;
BartJanssens 0:785cff1e5a7c 226 result[3] = elt[3] * s;
BartJanssens 0:785cff1e5a7c 227
BartJanssens 0:785cff1e5a7c 228 return(result);
BartJanssens 0:785cff1e5a7c 229 }
BartJanssens 0:785cff1e5a7c 230
BartJanssens 0:785cff1e5a7c 231 inline Quat Quat::operator / (Real s) const
BartJanssens 0:785cff1e5a7c 232 {
BartJanssens 0:785cff1e5a7c 233 Quat result;
BartJanssens 0:785cff1e5a7c 234
BartJanssens 0:785cff1e5a7c 235 result[0] = elt[0] / s;
BartJanssens 0:785cff1e5a7c 236 result[1] = elt[1] / s;
BartJanssens 0:785cff1e5a7c 237 result[2] = elt[2] / s;
BartJanssens 0:785cff1e5a7c 238 result[3] = elt[3] / s;
BartJanssens 0:785cff1e5a7c 239
BartJanssens 0:785cff1e5a7c 240 return(result);
BartJanssens 0:785cff1e5a7c 241 }
BartJanssens 0:785cff1e5a7c 242
BartJanssens 0:785cff1e5a7c 243 inline Quat operator * (Real s, const Quat &v)
BartJanssens 0:785cff1e5a7c 244 {
BartJanssens 0:785cff1e5a7c 245 return(v * s);
BartJanssens 0:785cff1e5a7c 246 }
BartJanssens 0:785cff1e5a7c 247
BartJanssens 0:785cff1e5a7c 248 // for convenience. Quat has no dot operation.
BartJanssens 0:785cff1e5a7c 249 inline Real dot(const Quat &a, const Quat &b)
BartJanssens 0:785cff1e5a7c 250 {
BartJanssens 0:785cff1e5a7c 251 return(a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]);
BartJanssens 0:785cff1e5a7c 252 }
BartJanssens 0:785cff1e5a7c 253
BartJanssens 0:785cff1e5a7c 254 inline Real len(const Quat &v)
BartJanssens 0:785cff1e5a7c 255 {
BartJanssens 0:785cff1e5a7c 256 return(sqrt(dot(v, v)));
BartJanssens 0:785cff1e5a7c 257 }
BartJanssens 0:785cff1e5a7c 258
BartJanssens 0:785cff1e5a7c 259 inline Real sqrlen(const Quat &v)
BartJanssens 0:785cff1e5a7c 260 {
BartJanssens 0:785cff1e5a7c 261 return(dot(v, v));
BartJanssens 0:785cff1e5a7c 262 }
BartJanssens 0:785cff1e5a7c 263
BartJanssens 0:785cff1e5a7c 264 inline Quat norm(const Quat &v)
BartJanssens 0:785cff1e5a7c 265 {
BartJanssens 0:785cff1e5a7c 266 Assert(sqrlen(v) > 0.0, "normalising length-zero vector");
BartJanssens 0:785cff1e5a7c 267 return(v / len(v));
BartJanssens 0:785cff1e5a7c 268 }
BartJanssens 0:785cff1e5a7c 269
BartJanssens 0:785cff1e5a7c 270 inline Void normalise(Quat &v)
BartJanssens 0:785cff1e5a7c 271 {
BartJanssens 0:785cff1e5a7c 272 v /= len(v);
BartJanssens 0:785cff1e5a7c 273 }
BartJanssens 0:785cff1e5a7c 274
BartJanssens 0:785cff1e5a7c 275 inline Quat slerp (const Quat& q1, const Quat& q2, Real t)
BartJanssens 0:785cff1e5a7c 276 {
BartJanssens 0:785cff1e5a7c 277 Quat result;
BartJanssens 0:785cff1e5a7c 278 Quat qq = q1;
BartJanssens 0:785cff1e5a7c 279
BartJanssens 0:785cff1e5a7c 280 if (dot(qq,q2) < 0)
BartJanssens 0:785cff1e5a7c 281 qq = -q1;
BartJanssens 0:785cff1e5a7c 282
BartJanssens 0:785cff1e5a7c 283 Real phi = acos(dot (qq, q2));
BartJanssens 0:785cff1e5a7c 284 Real denom = sin(phi);
BartJanssens 0:785cff1e5a7c 285
BartJanssens 0:785cff1e5a7c 286 result = sin(phi*(1-t))/denom * qq + sin(phi*t)/denom * q2;
BartJanssens 0:785cff1e5a7c 287
BartJanssens 0:785cff1e5a7c 288 return result;
BartJanssens 0:785cff1e5a7c 289 }
BartJanssens 0:785cff1e5a7c 290
BartJanssens 0:785cff1e5a7c 291 inline Quat conjugate(const Quat &q)
BartJanssens 0:785cff1e5a7c 292 {
BartJanssens 0:785cff1e5a7c 293 return Quat (q[0], -q[1], -q[2], -q[3]);
BartJanssens 0:785cff1e5a7c 294 }
BartJanssens 0:785cff1e5a7c 295
BartJanssens 0:785cff1e5a7c 296 #endif