Bart Janssens / SVL

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

Mat3.cpp@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 /*
BartJanssens 0:785cff1e5a7c 2 File: Mat3.cpp
BartJanssens 0:785cff1e5a7c 3
BartJanssens 0:785cff1e5a7c 4 Function: Implements Mat3.h
BartJanssens 0:785cff1e5a7c 5
BartJanssens 0:785cff1e5a7c 6 Author(s): Andrew Willmott
BartJanssens 0:785cff1e5a7c 7
BartJanssens 0:785cff1e5a7c 8 Copyright: (c) 1995-2001, Andrew Willmott
BartJanssens 0:785cff1e5a7c 9
BartJanssens 0:785cff1e5a7c 10 */
BartJanssens 0:785cff1e5a7c 11 #include "Mat3.h"
BartJanssens 0:785cff1e5a7c 12 #include "Vec4.h"
BartJanssens 0:785cff1e5a7c 13 //#include <cctype>
BartJanssens 0:785cff1e5a7c 14 //#include <iomanip>
BartJanssens 0:785cff1e5a7c 15
BartJanssens 0:785cff1e5a7c 16
BartJanssens 0:785cff1e5a7c 17 Mat3::Mat3(Real a, Real b, Real c,
BartJanssens 0:785cff1e5a7c 18 Real d, Real e, Real f,
BartJanssens 0:785cff1e5a7c 19 Real g, Real h, Real i)
BartJanssens 0:785cff1e5a7c 20 {
BartJanssens 0:785cff1e5a7c 21 row[0][0] = a; row[0][1] = b; row[0][2] = c;
BartJanssens 0:785cff1e5a7c 22 row[1][0] = d; row[1][1] = e; row[1][2] = f;
BartJanssens 0:785cff1e5a7c 23 row[2][0] = g; row[2][1] = h; row[2][2] = i;
BartJanssens 0:785cff1e5a7c 24 }
BartJanssens 0:785cff1e5a7c 25
BartJanssens 0:785cff1e5a7c 26 Mat3::Mat3(const Mat3 &m)
BartJanssens 0:785cff1e5a7c 27 {
BartJanssens 0:785cff1e5a7c 28 row[0] = m[0];
BartJanssens 0:785cff1e5a7c 29 row[1] = m[1];
BartJanssens 0:785cff1e5a7c 30 row[2] = m[2];
BartJanssens 0:785cff1e5a7c 31 }
BartJanssens 0:785cff1e5a7c 32
BartJanssens 0:785cff1e5a7c 33 Mat3 &Mat3::operator = (const Mat3 &m)
BartJanssens 0:785cff1e5a7c 34 {
BartJanssens 0:785cff1e5a7c 35 row[0] = m[0];
BartJanssens 0:785cff1e5a7c 36 row[1] = m[1];
BartJanssens 0:785cff1e5a7c 37 row[2] = m[2];
BartJanssens 0:785cff1e5a7c 38
BartJanssens 0:785cff1e5a7c 39 return(SELF);
BartJanssens 0:785cff1e5a7c 40 }
BartJanssens 0:785cff1e5a7c 41
BartJanssens 0:785cff1e5a7c 42 Mat3 &Mat3::operator += (const Mat3 &m)
BartJanssens 0:785cff1e5a7c 43 {
BartJanssens 0:785cff1e5a7c 44 row[0] += m[0];
BartJanssens 0:785cff1e5a7c 45 row[1] += m[1];
BartJanssens 0:785cff1e5a7c 46 row[2] += m[2];
BartJanssens 0:785cff1e5a7c 47
BartJanssens 0:785cff1e5a7c 48 return(SELF);
BartJanssens 0:785cff1e5a7c 49 }
BartJanssens 0:785cff1e5a7c 50
BartJanssens 0:785cff1e5a7c 51 Mat3 &Mat3::operator -= (const Mat3 &m)
BartJanssens 0:785cff1e5a7c 52 {
BartJanssens 0:785cff1e5a7c 53 row[0] -= m[0];
BartJanssens 0:785cff1e5a7c 54 row[1] -= m[1];
BartJanssens 0:785cff1e5a7c 55 row[2] -= m[2];
BartJanssens 0:785cff1e5a7c 56
BartJanssens 0:785cff1e5a7c 57 return(SELF);
BartJanssens 0:785cff1e5a7c 58 }
BartJanssens 0:785cff1e5a7c 59
BartJanssens 0:785cff1e5a7c 60 Mat3 &Mat3::operator *= (const Mat3 &m)
BartJanssens 0:785cff1e5a7c 61 {
BartJanssens 0:785cff1e5a7c 62 SELF = SELF * m;
BartJanssens 0:785cff1e5a7c 63
BartJanssens 0:785cff1e5a7c 64 return(SELF);
BartJanssens 0:785cff1e5a7c 65 }
BartJanssens 0:785cff1e5a7c 66
BartJanssens 0:785cff1e5a7c 67 Mat3 &Mat3::operator *= (Real s)
BartJanssens 0:785cff1e5a7c 68 {
BartJanssens 0:785cff1e5a7c 69 row[0] *= s;
BartJanssens 0:785cff1e5a7c 70 row[1] *= s;
BartJanssens 0:785cff1e5a7c 71 row[2] *= s;
BartJanssens 0:785cff1e5a7c 72
BartJanssens 0:785cff1e5a7c 73 return(SELF);
BartJanssens 0:785cff1e5a7c 74 }
BartJanssens 0:785cff1e5a7c 75
BartJanssens 0:785cff1e5a7c 76 Mat3 &Mat3::operator /= (Real s)
BartJanssens 0:785cff1e5a7c 77 {
BartJanssens 0:785cff1e5a7c 78 row[0] /= s;
BartJanssens 0:785cff1e5a7c 79 row[1] /= s;
BartJanssens 0:785cff1e5a7c 80 row[2] /= s;
BartJanssens 0:785cff1e5a7c 81
BartJanssens 0:785cff1e5a7c 82 return(SELF);
BartJanssens 0:785cff1e5a7c 83 }
BartJanssens 0:785cff1e5a7c 84
BartJanssens 0:785cff1e5a7c 85
BartJanssens 0:785cff1e5a7c 86 Bool Mat3::operator == (const Mat3 &m) const
BartJanssens 0:785cff1e5a7c 87 {
BartJanssens 0:785cff1e5a7c 88 return(row[0] == m[0] && row[1] == m[1] && row[2] == m[2]);
BartJanssens 0:785cff1e5a7c 89 }
BartJanssens 0:785cff1e5a7c 90
BartJanssens 0:785cff1e5a7c 91 Bool Mat3::operator != (const Mat3 &m) const
BartJanssens 0:785cff1e5a7c 92 {
BartJanssens 0:785cff1e5a7c 93 return(row[0] != m[0] || row[1] != m[1] || row[2] != m[2]);
BartJanssens 0:785cff1e5a7c 94 }
BartJanssens 0:785cff1e5a7c 95
BartJanssens 0:785cff1e5a7c 96
BartJanssens 0:785cff1e5a7c 97 Mat3 Mat3::operator + (const Mat3 &m) const
BartJanssens 0:785cff1e5a7c 98 {
BartJanssens 0:785cff1e5a7c 99 Mat3 result;
BartJanssens 0:785cff1e5a7c 100
BartJanssens 0:785cff1e5a7c 101 result[0] = row[0] + m[0];
BartJanssens 0:785cff1e5a7c 102 result[1] = row[1] + m[1];
BartJanssens 0:785cff1e5a7c 103 result[2] = row[2] + m[2];
BartJanssens 0:785cff1e5a7c 104
BartJanssens 0:785cff1e5a7c 105 return(result);
BartJanssens 0:785cff1e5a7c 106 }
BartJanssens 0:785cff1e5a7c 107
BartJanssens 0:785cff1e5a7c 108 Mat3 Mat3::operator - (const Mat3 &m) const
BartJanssens 0:785cff1e5a7c 109 {
BartJanssens 0:785cff1e5a7c 110 Mat3 result;
BartJanssens 0:785cff1e5a7c 111
BartJanssens 0:785cff1e5a7c 112 result[0] = row[0] - m[0];
BartJanssens 0:785cff1e5a7c 113 result[1] = row[1] - m[1];
BartJanssens 0:785cff1e5a7c 114 result[2] = row[2] - m[2];
BartJanssens 0:785cff1e5a7c 115
BartJanssens 0:785cff1e5a7c 116 return(result);
BartJanssens 0:785cff1e5a7c 117 }
BartJanssens 0:785cff1e5a7c 118
BartJanssens 0:785cff1e5a7c 119 Mat3 Mat3::operator - () const
BartJanssens 0:785cff1e5a7c 120 {
BartJanssens 0:785cff1e5a7c 121 Mat3 result;
BartJanssens 0:785cff1e5a7c 122
BartJanssens 0:785cff1e5a7c 123 result[0] = -row[0];
BartJanssens 0:785cff1e5a7c 124 result[1] = -row[1];
BartJanssens 0:785cff1e5a7c 125 result[2] = -row[2];
BartJanssens 0:785cff1e5a7c 126
BartJanssens 0:785cff1e5a7c 127 return(result);
BartJanssens 0:785cff1e5a7c 128 }
BartJanssens 0:785cff1e5a7c 129
BartJanssens 0:785cff1e5a7c 130 Mat3 Mat3::operator * (const Mat3 &m) const
BartJanssens 0:785cff1e5a7c 131 {
BartJanssens 0:785cff1e5a7c 132 #define N(x,y) row[x][y]
BartJanssens 0:785cff1e5a7c 133 #define M(x,y) m[x][y]
BartJanssens 0:785cff1e5a7c 134 #define R(x,y) result[x][y]
BartJanssens 0:785cff1e5a7c 135
BartJanssens 0:785cff1e5a7c 136 Mat3 result;
BartJanssens 0:785cff1e5a7c 137
BartJanssens 0:785cff1e5a7c 138 R(0,0) = N(0,0) * M(0,0) + N(0,1) * M(1,0) + N(0,2) * M(2,0);
BartJanssens 0:785cff1e5a7c 139 R(0,1) = N(0,0) * M(0,1) + N(0,1) * M(1,1) + N(0,2) * M(2,1);
BartJanssens 0:785cff1e5a7c 140 R(0,2) = N(0,0) * M(0,2) + N(0,1) * M(1,2) + N(0,2) * M(2,2);
BartJanssens 0:785cff1e5a7c 141
BartJanssens 0:785cff1e5a7c 142 R(1,0) = N(1,0) * M(0,0) + N(1,1) * M(1,0) + N(1,2) * M(2,0);
BartJanssens 0:785cff1e5a7c 143 R(1,1) = N(1,0) * M(0,1) + N(1,1) * M(1,1) + N(1,2) * M(2,1);
BartJanssens 0:785cff1e5a7c 144 R(1,2) = N(1,0) * M(0,2) + N(1,1) * M(1,2) + N(1,2) * M(2,2);
BartJanssens 0:785cff1e5a7c 145
BartJanssens 0:785cff1e5a7c 146 R(2,0) = N(2,0) * M(0,0) + N(2,1) * M(1,0) + N(2,2) * M(2,0);
BartJanssens 0:785cff1e5a7c 147 R(2,1) = N(2,0) * M(0,1) + N(2,1) * M(1,1) + N(2,2) * M(2,1);
BartJanssens 0:785cff1e5a7c 148 R(2,2) = N(2,0) * M(0,2) + N(2,1) * M(1,2) + N(2,2) * M(2,2);
BartJanssens 0:785cff1e5a7c 149
BartJanssens 0:785cff1e5a7c 150 return(result);
BartJanssens 0:785cff1e5a7c 151
BartJanssens 0:785cff1e5a7c 152 #undef N
BartJanssens 0:785cff1e5a7c 153 #undef M
BartJanssens 0:785cff1e5a7c 154 #undef R
BartJanssens 0:785cff1e5a7c 155 }
BartJanssens 0:785cff1e5a7c 156
BartJanssens 0:785cff1e5a7c 157 Mat3 Mat3::operator * (Real s) const
BartJanssens 0:785cff1e5a7c 158 {
BartJanssens 0:785cff1e5a7c 159 Mat3 result;
BartJanssens 0:785cff1e5a7c 160
BartJanssens 0:785cff1e5a7c 161 result[0] = row[0] * s;
BartJanssens 0:785cff1e5a7c 162 result[1] = row[1] * s;
BartJanssens 0:785cff1e5a7c 163 result[2] = row[2] * s;
BartJanssens 0:785cff1e5a7c 164
BartJanssens 0:785cff1e5a7c 165 return(result);
BartJanssens 0:785cff1e5a7c 166 }
BartJanssens 0:785cff1e5a7c 167
BartJanssens 0:785cff1e5a7c 168 Mat3 Mat3::operator / (Real s) const
BartJanssens 0:785cff1e5a7c 169 {
BartJanssens 0:785cff1e5a7c 170 Mat3 result;
BartJanssens 0:785cff1e5a7c 171
BartJanssens 0:785cff1e5a7c 172 result[0] = row[0] / s;
BartJanssens 0:785cff1e5a7c 173 result[1] = row[1] / s;
BartJanssens 0:785cff1e5a7c 174 result[2] = row[2] / s;
BartJanssens 0:785cff1e5a7c 175
BartJanssens 0:785cff1e5a7c 176 return(result);
BartJanssens 0:785cff1e5a7c 177 }
BartJanssens 0:785cff1e5a7c 178
BartJanssens 0:785cff1e5a7c 179 Mat3 trans(const Mat3 &m)
BartJanssens 0:785cff1e5a7c 180 {
BartJanssens 0:785cff1e5a7c 181 #define M(x,y) m[x][y]
BartJanssens 0:785cff1e5a7c 182 #define R(x,y) result[x][y]
BartJanssens 0:785cff1e5a7c 183
BartJanssens 0:785cff1e5a7c 184 Mat3 result;
BartJanssens 0:785cff1e5a7c 185
BartJanssens 0:785cff1e5a7c 186 R(0,0) = M(0,0); R(0,1) = M(1,0); R(0,2) = M(2,0);
BartJanssens 0:785cff1e5a7c 187 R(1,0) = M(0,1); R(1,1) = M(1,1); R(1,2) = M(2,1);
BartJanssens 0:785cff1e5a7c 188 R(2,0) = M(0,2); R(2,1) = M(1,2); R(2,2) = M(2,2);
BartJanssens 0:785cff1e5a7c 189
BartJanssens 0:785cff1e5a7c 190 return(result);
BartJanssens 0:785cff1e5a7c 191
BartJanssens 0:785cff1e5a7c 192 #undef M
BartJanssens 0:785cff1e5a7c 193 #undef R
BartJanssens 0:785cff1e5a7c 194 }
BartJanssens 0:785cff1e5a7c 195
BartJanssens 0:785cff1e5a7c 196 Mat3 adj(const Mat3 &m)
BartJanssens 0:785cff1e5a7c 197 {
BartJanssens 0:785cff1e5a7c 198 Mat3 result;
BartJanssens 0:785cff1e5a7c 199
BartJanssens 0:785cff1e5a7c 200 result[0] = cross(m[1], m[2]);
BartJanssens 0:785cff1e5a7c 201 result[1] = cross(m[2], m[0]);
BartJanssens 0:785cff1e5a7c 202 result[2] = cross(m[0], m[1]);
BartJanssens 0:785cff1e5a7c 203
BartJanssens 0:785cff1e5a7c 204 return(result);
BartJanssens 0:785cff1e5a7c 205 }
BartJanssens 0:785cff1e5a7c 206
BartJanssens 0:785cff1e5a7c 207
BartJanssens 0:785cff1e5a7c 208 Real trace(const Mat3 &m)
BartJanssens 0:785cff1e5a7c 209 {
BartJanssens 0:785cff1e5a7c 210 return(m[0][0] + m[1][1] + m[2][2]);
BartJanssens 0:785cff1e5a7c 211 }
BartJanssens 0:785cff1e5a7c 212
BartJanssens 0:785cff1e5a7c 213 Real det(const Mat3 &m)
BartJanssens 0:785cff1e5a7c 214 {
BartJanssens 0:785cff1e5a7c 215 return(dot(m[0], cross(m[1], m[2])));
BartJanssens 0:785cff1e5a7c 216 }
BartJanssens 0:785cff1e5a7c 217
BartJanssens 0:785cff1e5a7c 218 Mat3 inv(const Mat3 &m)
BartJanssens 0:785cff1e5a7c 219 {
BartJanssens 0:785cff1e5a7c 220 Real mDet;
BartJanssens 0:785cff1e5a7c 221 Mat3 adjoint;
BartJanssens 0:785cff1e5a7c 222 Mat3 result;
BartJanssens 0:785cff1e5a7c 223
BartJanssens 0:785cff1e5a7c 224 adjoint = adj(m);
BartJanssens 0:785cff1e5a7c 225 mDet = dot(adjoint[0], m[0]);
BartJanssens 0:785cff1e5a7c 226
BartJanssens 0:785cff1e5a7c 227 Assert(mDet != 0, "(Mat3::inv) matrix is non-singular");
BartJanssens 0:785cff1e5a7c 228
BartJanssens 0:785cff1e5a7c 229 result = trans(adjoint);
BartJanssens 0:785cff1e5a7c 230 result /= mDet;
BartJanssens 0:785cff1e5a7c 231
BartJanssens 0:785cff1e5a7c 232 return(result);
BartJanssens 0:785cff1e5a7c 233 }
BartJanssens 0:785cff1e5a7c 234
BartJanssens 0:785cff1e5a7c 235 Mat3 oprod(const Vec3 &a, const Vec3 &b)
BartJanssens 0:785cff1e5a7c 236 // returns outerproduct of a and b: a * trans(b)
BartJanssens 0:785cff1e5a7c 237 {
BartJanssens 0:785cff1e5a7c 238 Mat3 result;
BartJanssens 0:785cff1e5a7c 239
BartJanssens 0:785cff1e5a7c 240 result[0] = a[0] * b;
BartJanssens 0:785cff1e5a7c 241 result[1] = a[1] * b;
BartJanssens 0:785cff1e5a7c 242 result[2] = a[2] * b;
BartJanssens 0:785cff1e5a7c 243
BartJanssens 0:785cff1e5a7c 244 return(result);
BartJanssens 0:785cff1e5a7c 245 }
BartJanssens 0:785cff1e5a7c 246
BartJanssens 0:785cff1e5a7c 247 Void Mat3::MakeZero()
BartJanssens 0:785cff1e5a7c 248 {
BartJanssens 0:785cff1e5a7c 249 Int i;
BartJanssens 0:785cff1e5a7c 250
BartJanssens 0:785cff1e5a7c 251 for (i = 0; i < 9; i++)
BartJanssens 0:785cff1e5a7c 252 ((Real*) row)[i] = vl_zero;
BartJanssens 0:785cff1e5a7c 253 }
BartJanssens 0:785cff1e5a7c 254
BartJanssens 0:785cff1e5a7c 255 Void Mat3::MakeDiag(Real k)
BartJanssens 0:785cff1e5a7c 256 {
BartJanssens 0:785cff1e5a7c 257 Int i, j;
BartJanssens 0:785cff1e5a7c 258
BartJanssens 0:785cff1e5a7c 259 for (i = 0; i < 3; i++)
BartJanssens 0:785cff1e5a7c 260 for (j = 0; j < 3; j++)
BartJanssens 0:785cff1e5a7c 261 if (i == j)
BartJanssens 0:785cff1e5a7c 262 row[i][j] = k;
BartJanssens 0:785cff1e5a7c 263 else
BartJanssens 0:785cff1e5a7c 264 row[i][j] = vl_zero;
BartJanssens 0:785cff1e5a7c 265 }
BartJanssens 0:785cff1e5a7c 266
BartJanssens 0:785cff1e5a7c 267 Void Mat3::MakeBlock(Real k)
BartJanssens 0:785cff1e5a7c 268 {
BartJanssens 0:785cff1e5a7c 269 Int i;
BartJanssens 0:785cff1e5a7c 270
BartJanssens 0:785cff1e5a7c 271 for (i = 0; i < 9; i++)
BartJanssens 0:785cff1e5a7c 272 ((Real *) row)[i] = k;
BartJanssens 0:785cff1e5a7c 273 }
BartJanssens 0:785cff1e5a7c 274
BartJanssens 0:785cff1e5a7c 275 void printMat3(const Mat3 &m)
BartJanssens 0:785cff1e5a7c 276 {
BartJanssens 0:785cff1e5a7c 277 printf("[");
BartJanssens 0:785cff1e5a7c 278 printVec3(m[0]);
BartJanssens 0:785cff1e5a7c 279 printf("\r\n");
BartJanssens 0:785cff1e5a7c 280 printVec3(m[1]);
BartJanssens 0:785cff1e5a7c 281 printf("\r\n");
BartJanssens 0:785cff1e5a7c 282 printVec3(m[2]);
BartJanssens 0:785cff1e5a7c 283 printf("]\r\n");
BartJanssens 0:785cff1e5a7c 284 }
BartJanssens 0:785cff1e5a7c 285
BartJanssens 0:785cff1e5a7c 286 /*
BartJanssens 0:785cff1e5a7c 287 ostream &operator << (ostream &s, const Mat3 &m)
BartJanssens 0:785cff1e5a7c 288 {
BartJanssens 0:785cff1e5a7c 289 Int w = s.width();
BartJanssens 0:785cff1e5a7c 290
BartJanssens 0:785cff1e5a7c 291 return(s << '[' << m[0] << "\r\n" << setw(w) << m[1] << "\r\n" << setw(w)
BartJanssens 0:785cff1e5a7c 292 << m[2] << ']' << "\r\n");
BartJanssens 0:785cff1e5a7c 293 }
BartJanssens 0:785cff1e5a7c 294
BartJanssens 0:785cff1e5a7c 295 istream &operator >> (istream &s, Mat3 &m)
BartJanssens 0:785cff1e5a7c 296 {
BartJanssens 0:785cff1e5a7c 297 Mat3 result;
BartJanssens 0:785cff1e5a7c 298 Char c;
BartJanssens 0:785cff1e5a7c 299
BartJanssens 0:785cff1e5a7c 300 // Expected format: [[1 2 3] [4 5 6] [7 8 9]]
BartJanssens 0:785cff1e5a7c 301 // Each vector is a column of the matrix.
BartJanssens 0:785cff1e5a7c 302
BartJanssens 0:785cff1e5a7c 303 while (s >> c && isspace(c)) // ignore leading white space
BartJanssens 0:785cff1e5a7c 304 ;
BartJanssens 0:785cff1e5a7c 305
BartJanssens 0:785cff1e5a7c 306 if (c == '[')
BartJanssens 0:785cff1e5a7c 307 {
BartJanssens 0:785cff1e5a7c 308 s >> result[0] >> result[1] >> result[2];
BartJanssens 0:785cff1e5a7c 309
BartJanssens 0:785cff1e5a7c 310 if (!s)
BartJanssens 0:785cff1e5a7c 311 {
BartJanssens 0:785cff1e5a7c 312 cerr << "Expected number while reading matrix\n";
BartJanssens 0:785cff1e5a7c 313 return(s);
BartJanssens 0:785cff1e5a7c 314 }
BartJanssens 0:785cff1e5a7c 315
BartJanssens 0:785cff1e5a7c 316 while (s >> c && isspace(c))
BartJanssens 0:785cff1e5a7c 317 ;
BartJanssens 0:785cff1e5a7c 318
BartJanssens 0:785cff1e5a7c 319 if (c != ']')
BartJanssens 0:785cff1e5a7c 320 {
BartJanssens 0:785cff1e5a7c 321 s.clear(ios::failbit);
BartJanssens 0:785cff1e5a7c 322 cerr << "Expected ']' while reading matrix\n";
BartJanssens 0:785cff1e5a7c 323 return(s);
BartJanssens 0:785cff1e5a7c 324 }
BartJanssens 0:785cff1e5a7c 325 }
BartJanssens 0:785cff1e5a7c 326 else
BartJanssens 0:785cff1e5a7c 327 {
BartJanssens 0:785cff1e5a7c 328 s.clear(ios::failbit);
BartJanssens 0:785cff1e5a7c 329 cerr << "Expected '[' while reading matrix\n";
BartJanssens 0:785cff1e5a7c 330 return(s);
BartJanssens 0:785cff1e5a7c 331 }
BartJanssens 0:785cff1e5a7c 332
BartJanssens 0:785cff1e5a7c 333 m = result;
BartJanssens 0:785cff1e5a7c 334 return(s);
BartJanssens 0:785cff1e5a7c 335 }
BartJanssens 0:785cff1e5a7c 336 */
BartJanssens 0:785cff1e5a7c 337
BartJanssens 0:785cff1e5a7c 338
BartJanssens 0:785cff1e5a7c 339 Mat3 &Mat3::MakeRot(const Vec4 &q)
BartJanssens 0:785cff1e5a7c 340 // modify to the new quat format [q0, (q1,q2,q3)]
BartJanssens 0:785cff1e5a7c 341 {
BartJanssens 0:785cff1e5a7c 342 Real i2 = 2 * q[1],
BartJanssens 0:785cff1e5a7c 343 j2 = 2 * q[2],
BartJanssens 0:785cff1e5a7c 344 k2 = 2 * q[3],
BartJanssens 0:785cff1e5a7c 345 ij = i2 * q[2],
BartJanssens 0:785cff1e5a7c 346 ik = i2 * q[3],
BartJanssens 0:785cff1e5a7c 347 jk = j2 * q[3],
BartJanssens 0:785cff1e5a7c 348 ri = i2 * q[0],
BartJanssens 0:785cff1e5a7c 349 rj = j2 * q[0],
BartJanssens 0:785cff1e5a7c 350 rk = k2 * q[0];
BartJanssens 0:785cff1e5a7c 351
BartJanssens 0:785cff1e5a7c 352 i2 *= q[1];
BartJanssens 0:785cff1e5a7c 353 j2 *= q[2];
BartJanssens 0:785cff1e5a7c 354 k2 *= q[3];
BartJanssens 0:785cff1e5a7c 355
BartJanssens 0:785cff1e5a7c 356 #if VL_ROW_ORIENT // off by default
BartJanssens 0:785cff1e5a7c 357 row[0][0] = 1 - j2 - k2; row[0][1] = ij + rk ; row[0][2] = ik - rj;
BartJanssens 0:785cff1e5a7c 358 row[1][0] = ij - rk ; row[1][1] = 1 - i2 - k2; row[1][2] = jk + ri;
BartJanssens 0:785cff1e5a7c 359 row[2][0] = ik + rj ; row[2][1] = jk - ri ; row[2][2] = 1 - i2 - j2;
BartJanssens 0:785cff1e5a7c 360 #else
BartJanssens 0:785cff1e5a7c 361 row[0][0] = 1 - j2 - k2; row[0][1] = ij - rk ; row[0][2] = ik + rj;
BartJanssens 0:785cff1e5a7c 362 row[1][0] = ij + rk ; row[1][1] = 1 - i2 - k2; row[1][2] = jk - ri;
BartJanssens 0:785cff1e5a7c 363 row[2][0] = ik - rj ; row[2][1] = jk + ri ; row[2][2] = 1 - i2 - j2;
BartJanssens 0:785cff1e5a7c 364 #endif
BartJanssens 0:785cff1e5a7c 365
BartJanssens 0:785cff1e5a7c 366 return(SELF);
BartJanssens 0:785cff1e5a7c 367 }
BartJanssens 0:785cff1e5a7c 368
BartJanssens 0:785cff1e5a7c 369 Mat3 &Mat3::MakeRot(const Vec3 &axis, Real theta)
BartJanssens 0:785cff1e5a7c 370 // modify to the new quat format [q0,(q1,q2,q3)]
BartJanssens 0:785cff1e5a7c 371 {
BartJanssens 0:785cff1e5a7c 372 Real s;
BartJanssens 0:785cff1e5a7c 373 Vec4 q;
BartJanssens 0:785cff1e5a7c 374
BartJanssens 0:785cff1e5a7c 375 theta /= 2.0;
BartJanssens 0:785cff1e5a7c 376 s = sin(theta);
BartJanssens 0:785cff1e5a7c 377
BartJanssens 0:785cff1e5a7c 378 q[1] = s * axis[0];
BartJanssens 0:785cff1e5a7c 379 q[2] = s * axis[1];
BartJanssens 0:785cff1e5a7c 380 q[3] = s * axis[2];
BartJanssens 0:785cff1e5a7c 381 q[0] = cos(theta);
BartJanssens 0:785cff1e5a7c 382
BartJanssens 0:785cff1e5a7c 383 MakeRot(q);
BartJanssens 0:785cff1e5a7c 384
BartJanssens 0:785cff1e5a7c 385 return(SELF);
BartJanssens 0:785cff1e5a7c 386 }
BartJanssens 0:785cff1e5a7c 387
BartJanssens 0:785cff1e5a7c 388 Mat3 &Mat3::MakeScale(const Vec3 &s)
BartJanssens 0:785cff1e5a7c 389 {
BartJanssens 0:785cff1e5a7c 390 MakeZero();
BartJanssens 0:785cff1e5a7c 391
BartJanssens 0:785cff1e5a7c 392 row[0][0] = s[0];
BartJanssens 0:785cff1e5a7c 393 row[1][1] = s[1];
BartJanssens 0:785cff1e5a7c 394 row[2][2] = s[2];
BartJanssens 0:785cff1e5a7c 395
BartJanssens 0:785cff1e5a7c 396 return(SELF);
BartJanssens 0:785cff1e5a7c 397 }
BartJanssens 0:785cff1e5a7c 398
BartJanssens 0:785cff1e5a7c 399 Mat3 &Mat3::MakeHRot(Real theta)
BartJanssens 0:785cff1e5a7c 400 {
BartJanssens 0:785cff1e5a7c 401 Real c, s;
BartJanssens 0:785cff1e5a7c 402
BartJanssens 0:785cff1e5a7c 403 MakeDiag();
BartJanssens 0:785cff1e5a7c 404
BartJanssens 0:785cff1e5a7c 405 s = sin(theta);
BartJanssens 0:785cff1e5a7c 406 c = cos(theta);
BartJanssens 0:785cff1e5a7c 407
BartJanssens 0:785cff1e5a7c 408 #ifdef VL_ROW_ORIENT
BartJanssens 0:785cff1e5a7c 409 row[0][0] = c; row[0][1] = s;
BartJanssens 0:785cff1e5a7c 410 row[1][0] = -s; row[1][1] = c;
BartJanssens 0:785cff1e5a7c 411 #else
BartJanssens 0:785cff1e5a7c 412 row[0][0] = c; row[0][1] = -s;
BartJanssens 0:785cff1e5a7c 413 row[1][0] = s; row[1][1] = c;
BartJanssens 0:785cff1e5a7c 414 #endif
BartJanssens 0:785cff1e5a7c 415
BartJanssens 0:785cff1e5a7c 416 return(SELF);
BartJanssens 0:785cff1e5a7c 417 }
BartJanssens 0:785cff1e5a7c 418
BartJanssens 0:785cff1e5a7c 419 Mat3 &Mat3::MakeHScale(const Vec2 &s)
BartJanssens 0:785cff1e5a7c 420 {
BartJanssens 0:785cff1e5a7c 421 MakeDiag();
BartJanssens 0:785cff1e5a7c 422
BartJanssens 0:785cff1e5a7c 423 row[0][0] = s[0];
BartJanssens 0:785cff1e5a7c 424 row[1][1] = s[1];
BartJanssens 0:785cff1e5a7c 425
BartJanssens 0:785cff1e5a7c 426 return(SELF);
BartJanssens 0:785cff1e5a7c 427 }
BartJanssens 0:785cff1e5a7c 428
BartJanssens 0:785cff1e5a7c 429 Mat3 &Mat3::MakeHTrans(const Vec2 &t)
BartJanssens 0:785cff1e5a7c 430 {
BartJanssens 0:785cff1e5a7c 431 MakeDiag();
BartJanssens 0:785cff1e5a7c 432
BartJanssens 0:785cff1e5a7c 433 #ifdef VL_ROW_ORIENT
BartJanssens 0:785cff1e5a7c 434 row[2][0] = t[0];
BartJanssens 0:785cff1e5a7c 435 row[2][1] = t[1];
BartJanssens 0:785cff1e5a7c 436 #else
BartJanssens 0:785cff1e5a7c 437 row[0][2] = t[0];
BartJanssens 0:785cff1e5a7c 438 row[1][2] = t[1];
BartJanssens 0:785cff1e5a7c 439 #endif
BartJanssens 0:785cff1e5a7c 440
BartJanssens 0:785cff1e5a7c 441 return(SELF);
BartJanssens 0:785cff1e5a7c 442 }