Bart Janssens / SVL

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

Quat.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 #include "Quat.h"
BartJanssens 0:785cff1e5a7c 2 //#include "svl/Mat3.h"
BartJanssens 0:785cff1e5a7c 3 #include <cctype>
BartJanssens 0:785cff1e5a7c 4 #include <iomanip>
BartJanssens 0:785cff1e5a7c 5
BartJanssens 0:785cff1e5a7c 6 //
BartJanssens 0:785cff1e5a7c 7 // Quat in SVL: [(q0,q1,q2),q3]
BartJanssens 0:785cff1e5a7c 8 // update:
BartJanssens 0:785cff1e5a7c 9 // Modify to conform with most literature:
BartJanssens 0:785cff1e5a7c 10 // Quat in SVL [q0, (q1,q2,q3)]
BartJanssens 0:785cff1e5a7c 11 // (may need to modify other SVL files relating to quat!!!
BartJanssens 0:785cff1e5a7c 12 //
BartJanssens 0:785cff1e5a7c 13 // implement:
BartJanssens 0:785cff1e5a7c 14 //
BartJanssens 0:785cff1e5a7c 15 // Quaterion multiplication
BartJanssens 0:785cff1e5a7c 16 // 3x3 -> mQuat;
BartJanssens 0:785cff1e5a7c 17 // mQuat->3x3 (NY) .... already available
BartJanssens 0:785cff1e5a7c 18 // slerp
BartJanssens 0:785cff1e5a7c 19 //
BartJanssens 0:785cff1e5a7c 20 // find application that demonstrate the needs ...
BartJanssens 0:785cff1e5a7c 21 //
BartJanssens 0:785cff1e5a7c 22
BartJanssens 0:785cff1e5a7c 23 Mat3 Rot3 (const Quat &q)
BartJanssens 0:785cff1e5a7c 24 {
BartJanssens 0:785cff1e5a7c 25 Mat3 result;
BartJanssens 0:785cff1e5a7c 26 Vec4 v(q[0],q[1],q[2],q[3]);
BartJanssens 0:785cff1e5a7c 27 result.MakeRot(v);
BartJanssens 0:785cff1e5a7c 28 return(result);
BartJanssens 0:785cff1e5a7c 29 }
BartJanssens 0:785cff1e5a7c 30
BartJanssens 0:785cff1e5a7c 31 Mat4 HRot4 (const Quat &q)
BartJanssens 0:785cff1e5a7c 32 {
BartJanssens 0:785cff1e5a7c 33 Mat4 result;
BartJanssens 0:785cff1e5a7c 34 Vec4 v(q[0],q[1],q[2],q[3]);
BartJanssens 0:785cff1e5a7c 35 result.MakeHRot(v);
BartJanssens 0:785cff1e5a7c 36 return(result);
BartJanssens 0:785cff1e5a7c 37 }
BartJanssens 0:785cff1e5a7c 38
BartJanssens 0:785cff1e5a7c 39 Quat::Quat(const Mat3 & m)
BartJanssens 0:785cff1e5a7c 40 {
BartJanssens 0:785cff1e5a7c 41 Real compare[4], max;
BartJanssens 0:785cff1e5a7c 42 compare[0] = 1 + m[0][0] + m[1][1] + m[2][2];
BartJanssens 0:785cff1e5a7c 43 compare[1] = 1 + m[0][0] - m[1][1] - m[2][2];
BartJanssens 0:785cff1e5a7c 44 compare[2] = 1 - m[0][0] + m[1][1] - m[2][2];
BartJanssens 0:785cff1e5a7c 45 compare[3] = 1 - m[0][0] - m[1][1] + m[2][2];
BartJanssens 0:785cff1e5a7c 46
BartJanssens 0:785cff1e5a7c 47 int i, which;
BartJanssens 0:785cff1e5a7c 48 for (i = 0, which = -1, max = 0; i < 4; i++)
BartJanssens 0:785cff1e5a7c 49 if (fabs(compare[i]) > max)
BartJanssens 0:785cff1e5a7c 50 which = i, max = fabs(compare[i]);
BartJanssens 0:785cff1e5a7c 51
BartJanssens 0:785cff1e5a7c 52 Real q0,q1,q2,q3;
BartJanssens 0:785cff1e5a7c 53 switch (which) {
BartJanssens 0:785cff1e5a7c 54 case 0:
BartJanssens 0:785cff1e5a7c 55 q0 = 0.5*sqrt (compare[which]);
BartJanssens 0:785cff1e5a7c 56 q1 = 0.25*(m[2][1] - m[1][2])/q0;
BartJanssens 0:785cff1e5a7c 57 q2 = 0.25*(m[0][2] - m[2][0])/q0;
BartJanssens 0:785cff1e5a7c 58 q3 = 0.25*(m[1][0] - m[0][1])/q0;
BartJanssens 0:785cff1e5a7c 59 break;
BartJanssens 0:785cff1e5a7c 60
BartJanssens 0:785cff1e5a7c 61 case 1:
BartJanssens 0:785cff1e5a7c 62 q1 = 0.5*sqrt (compare[which]);
BartJanssens 0:785cff1e5a7c 63 q0 = 0.25*(m[2][1] - m[1][2])/q1;
BartJanssens 0:785cff1e5a7c 64 q2 = 0.25*(m[0][1] + m[1][0])/q1;
BartJanssens 0:785cff1e5a7c 65 q3 = 0.25*(m[0][2] + m[2][0])/q1;
BartJanssens 0:785cff1e5a7c 66 break;
BartJanssens 0:785cff1e5a7c 67
BartJanssens 0:785cff1e5a7c 68 case 2:
BartJanssens 0:785cff1e5a7c 69 q2 = 0.5*sqrt (compare[which]);
BartJanssens 0:785cff1e5a7c 70 q0 = 0.25*(m[0][2] - m[2][0])/q2;
BartJanssens 0:785cff1e5a7c 71 q1 = 0.25*(m[0][1] + m[1][0])/q2;
BartJanssens 0:785cff1e5a7c 72 q3 = 0.25*(m[1][2] + m[2][1])/q2;
BartJanssens 0:785cff1e5a7c 73 break;
BartJanssens 0:785cff1e5a7c 74
BartJanssens 0:785cff1e5a7c 75 case 3:
BartJanssens 0:785cff1e5a7c 76 q3 = 0.5*sqrt (compare[which]);
BartJanssens 0:785cff1e5a7c 77 q0 = 0.25*(m[1][0] - m[0][1])/q3;
BartJanssens 0:785cff1e5a7c 78 q1 = 0.25*(m[0][2] + m[2][0])/q3;
BartJanssens 0:785cff1e5a7c 79 q2 = 0.25*(m[1][2] + m[2][1])/q3;
BartJanssens 0:785cff1e5a7c 80 break;
BartJanssens 0:785cff1e5a7c 81 }
BartJanssens 0:785cff1e5a7c 82 elt[1] = q1, elt[2] = q2, elt[3] = q3, elt[0] = q0;
BartJanssens 0:785cff1e5a7c 83 }
BartJanssens 0:785cff1e5a7c 84
BartJanssens 0:785cff1e5a7c 85 void printQuat(const Quat &v)
BartJanssens 0:785cff1e5a7c 86 {
BartJanssens 0:785cff1e5a7c 87 Int i;
BartJanssens 0:785cff1e5a7c 88
BartJanssens 0:785cff1e5a7c 89 printf("[");
BartJanssens 0:785cff1e5a7c 90 for (i = 0; i < 3; i++){
BartJanssens 0:785cff1e5a7c 91 printf("%f", v[i]);
BartJanssens 0:785cff1e5a7c 92 printf(" ");
BartJanssens 0:785cff1e5a7c 93 }
BartJanssens 0:785cff1e5a7c 94 printf("%f ]/r/n", v[3]);
BartJanssens 0:785cff1e5a7c 95 }
BartJanssens 0:785cff1e5a7c 96
BartJanssens 0:785cff1e5a7c 97 /*
BartJanssens 0:785cff1e5a7c 98 ostream &operator << (ostream &s, const Quat &v)
BartJanssens 0:785cff1e5a7c 99 {
BartJanssens 0:785cff1e5a7c 100 Int w = s.width();
BartJanssens 0:785cff1e5a7c 101
BartJanssens 0:785cff1e5a7c 102 return(s << '[' << v[0] << ' ' << setw(w) << v[1] << ' '
BartJanssens 0:785cff1e5a7c 103 << setw(w) << v[2] << ' ' << setw(w) << v[3] << ']');
BartJanssens 0:785cff1e5a7c 104 }
BartJanssens 0:785cff1e5a7c 105
BartJanssens 0:785cff1e5a7c 106 istream &operator >> (istream &s, Quat &v)
BartJanssens 0:785cff1e5a7c 107 {
BartJanssens 0:785cff1e5a7c 108 Quat result;
BartJanssens 0:785cff1e5a7c 109 Char c;
BartJanssens 0:785cff1e5a7c 110
BartJanssens 0:785cff1e5a7c 111 // Expected format: [1 2 3 4]
BartJanssens 0:785cff1e5a7c 112
BartJanssens 0:785cff1e5a7c 113 while (s >> c && isspace(c))
BartJanssens 0:785cff1e5a7c 114 ;
BartJanssens 0:785cff1e5a7c 115
BartJanssens 0:785cff1e5a7c 116 if (c == '[')
BartJanssens 0:785cff1e5a7c 117 {
BartJanssens 0:785cff1e5a7c 118 s >> result[0] >> result[1] >> result[2] >> result[3];
BartJanssens 0:785cff1e5a7c 119
BartJanssens 0:785cff1e5a7c 120 if (!s)
BartJanssens 0:785cff1e5a7c 121 {
BartJanssens 0:785cff1e5a7c 122 cerr << "Error: Expected number while reading vector\n";
BartJanssens 0:785cff1e5a7c 123 return(s);
BartJanssens 0:785cff1e5a7c 124 }
BartJanssens 0:785cff1e5a7c 125
BartJanssens 0:785cff1e5a7c 126 while (s >> c && isspace(c))
BartJanssens 0:785cff1e5a7c 127 ;
BartJanssens 0:785cff1e5a7c 128
BartJanssens 0:785cff1e5a7c 129 if (c != ']')
BartJanssens 0:785cff1e5a7c 130 {
BartJanssens 0:785cff1e5a7c 131 s.clear(ios::failbit);
BartJanssens 0:785cff1e5a7c 132 cerr << "Error: Expected ']' while reading vector\n";
BartJanssens 0:785cff1e5a7c 133 return(s);
BartJanssens 0:785cff1e5a7c 134 }
BartJanssens 0:785cff1e5a7c 135 }
BartJanssens 0:785cff1e5a7c 136 else
BartJanssens 0:785cff1e5a7c 137 {
BartJanssens 0:785cff1e5a7c 138 s.clear(ios::failbit);
BartJanssens 0:785cff1e5a7c 139 cerr << "Error: Expected '[' while reading vector\n";
BartJanssens 0:785cff1e5a7c 140 return(s);
BartJanssens 0:785cff1e5a7c 141 }
BartJanssens 0:785cff1e5a7c 142
BartJanssens 0:785cff1e5a7c 143 v = result;
BartJanssens 0:785cff1e5a7c 144 return(s);
BartJanssens 0:785cff1e5a7c 145 }
BartJanssens 0:785cff1e5a7c 146 */