Simple Vector Library 1.5 http://www.cs.cmu.edu/~ajw/doc/svl.html
Quat.cpp@0:785cff1e5a7c, 2016-01-04 (annotated)
- Committer:
- BartJanssens
- Date:
- Mon Jan 04 15:19:10 2016 +0000
- Revision:
- 0:785cff1e5a7c
svl-1.5
Who changed what in which revision?
User | Revision | Line number | New 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 | */ |