source: vtkvis/trunk/Math.h @ 4952

Last change on this file since 4952 was 3744, checked in by ldelgass, 11 years ago

add convenience functions

  • Property svn:eol-style set to native
File size: 5.6 KB
Line 
1/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*- */
2/*
3 * Copyright (C) 2004-2012  HUBzero Foundation, LLC
4 *
5 * Author: Leif Delgass <ldelgass@purdue.edu>
6 */
7
8#ifndef VTKVIS_MATH_H
9#define VTKVIS_MATH_H
10
11#include <cmath>
12#include <cstring>
13
14#include <vtkMath.h>
15#include <vtkMatrix4x4.h>
16
17namespace VtkVis {
18
19/**
20 * \brief Convert a quaternion to an axis/angle rotation
21 *
22 * \param[in] quat Quaternion with scalar first: wxyz
23 * \param[out] angleAxis axis/angle rotation with angle in degrees first
24 */
25inline void quatToAngleAxis(const double quat[4], double angleAxis[4])
26{
27    angleAxis[0] = vtkMath::DegreesFromRadians(2.0 * acos(quat[0]));
28    if (angleAxis[0] < 1.0e-6) {
29        angleAxis[1] = 1;
30        angleAxis[2] = 0;
31        angleAxis[3] = 0;
32    } else {
33        double denom = sqrt(1. - quat[0] * quat[0]);
34        angleAxis[1] = quat[1] / denom;
35        angleAxis[2] = quat[2] / denom;
36        angleAxis[3] = quat[3] / denom;
37    }
38}
39
40/**
41 * \brief Convert an axis/angle rotation to a quaternion
42 *
43 * \param[in] angleAxis axis/angle rotation with angle in degrees first
44 * \param[out] quat Quaternion with scalar first: wxyz
45 */
46inline void angleAxisToQuat(const double angleAxis[4], double quat[4])
47{
48    quat[0] = cos(vtkMath::RadiansFromDegrees(angleAxis[0]) / 2.0);
49    double sinHalfAngle = sin(vtkMath::RadiansFromDegrees(angleAxis[0]) / 2.0);
50    quat[1] = angleAxis[1] * sinHalfAngle;
51    quat[2] = angleAxis[2] * sinHalfAngle;
52    quat[3] = angleAxis[3] * sinHalfAngle;
53}
54
55/**
56 * \brief Fill a vtkMatrix4x4 from a quaternion
57 *
58 * \param[in] quat Quaternion with scalar first: wxyz
59 * \param[out] mat Matrix to be filled in
60 */
61inline void quaternionToMatrix4x4(const double quat[4], vtkMatrix4x4& mat)
62{
63    double ww = quat[0]*quat[0];
64    double wx = quat[0]*quat[1];
65    double wy = quat[0]*quat[2];
66    double wz = quat[0]*quat[3];
67
68    double xx = quat[1]*quat[1];
69    double yy = quat[2]*quat[2];
70    double zz = quat[3]*quat[3];
71
72    double xy = quat[1]*quat[2];
73    double xz = quat[1]*quat[3];
74    double yz = quat[2]*quat[3];
75
76    double rr = xx + yy + zz;
77    // normalization factor, just in case quaternion was not normalized
78    double f = double(1)/double(sqrt(ww + rr));
79    double s = (ww - rr)*f;
80    f *= 2;
81
82    mat[0][0] = xx*f + s;
83    mat[1][0] = (xy + wz)*f;
84    mat[2][0] = (xz - wy)*f;
85
86    mat[0][1] = (xy - wz)*f;
87    mat[1][1] = yy*f + s;
88    mat[2][1] = (yz + wx)*f;
89
90    mat[0][2] = (xz + wy)*f;
91    mat[1][2] = (yz - wx)*f;
92    mat[2][2] = zz*f + s;
93}
94
95/**
96 * \brief Fill a vtkMatrix4x4 from a quaternion, but with the matrix
97 * transposed.
98 *
99 * \param[in] quat Quaternion with scalar first: wxyz
100 * \param[out] mat Matrix to be filled in
101 */
102inline void quaternionToTransposeMatrix4x4(const double quat[4], vtkMatrix4x4& mat)
103{
104    double ww = quat[0]*quat[0];
105    double wx = quat[0]*quat[1];
106    double wy = quat[0]*quat[2];
107    double wz = quat[0]*quat[3];
108
109    double xx = quat[1]*quat[1];
110    double yy = quat[2]*quat[2];
111    double zz = quat[3]*quat[3];
112
113    double xy = quat[1]*quat[2];
114    double xz = quat[1]*quat[3];
115    double yz = quat[2]*quat[3];
116
117    double rr = xx + yy + zz;
118    // normalization factor, just in case quaternion was not normalized
119    double f = double(1)/double(sqrt(ww + rr));
120    double s = (ww - rr)*f;
121    f *= 2;
122
123    mat[0][0] = xx*f + s;
124    mat[0][1] = (xy + wz)*f;
125    mat[0][2] = (xz - wy)*f;
126
127    mat[1][0] = (xy - wz)*f;
128    mat[1][1] = yy*f + s;
129    mat[1][2] = (yz + wx)*f;
130
131    mat[2][0] = (xz + wy)*f;
132    mat[2][1] = (yz - wx)*f;
133    mat[2][2] = zz*f + s;
134}
135
136/**
137 * \brief Multiply two quaternions and return the result
138 *
139 * Note: result can be the same memory location as one of the
140 * inputs
141 */
142inline double *quatMult(const double q1[4], const double q2[4], double result[4])
143{
144    double q1w = q1[0];
145    double q1x = q1[1];
146    double q1y = q1[2];
147    double q1z = q1[3];
148    double q2w = q2[0];
149    double q2x = q2[1];
150    double q2y = q2[2];
151    double q2z = q2[3];
152    result[0] = (q1w*q2w) - (q1x*q2x) - (q1y*q2y) - (q1z*q2z);
153    result[1] = (q1w*q2x) + (q1x*q2w) + (q1y*q2z) - (q1z*q2y);
154    result[2] = (q1w*q2y) + (q1y*q2w) + (q1z*q2x) - (q1x*q2z);
155    result[3] = (q1w*q2z) + (q1z*q2w) + (q1x*q2y) - (q1y*q2x);
156    return result;
157}
158
159/**
160 * \brief Compute the conjugate of a quaternion
161 *
162 * Note: result can be the same memory location as the input
163 */
164inline double *quatConjugate(const double quat[4], double result[4])
165{
166    if (result != quat)
167        result[0] = quat[0];
168    result[1] = -quat[1];
169    result[2] = -quat[2];
170    result[3] = -quat[3];
171    return result;
172}
173
174/**
175 * \brief Compute the reciprocal of a quaternion
176 *
177 * Note: result can be the same memory location as the input
178 */
179inline double *quatReciprocal(const double quat[4], double result[4])
180{
181    double denom =
182        quat[0]*quat[0] +
183        quat[1]*quat[1] +
184        quat[2]*quat[2] +
185        quat[3]*quat[3];
186    VtkVis::quatConjugate(quat, result);
187    for (int i = 0; i < 4; i++) {
188        result[i] /= denom;
189    }
190    return result;
191}
192
193/**
194 * \brief Compute the reciprocal of a quaternion in place
195 */
196inline double *quatReciprocal(double quat[4])
197{
198    return VtkVis::quatReciprocal(quat, quat);
199}
200
201/**
202 * \brief Deep copy a quaternion
203 */
204inline void copyQuat(const double quat[4], double result[4])
205{
206    memcpy(result, quat, sizeof(double)*4);
207}
208
209inline double min2(double a, double b)
210{
211    return ((a < b) ? a : b);
212}
213
214inline double max2(double a, double b)
215{
216    return ((a > b) ? a : b);
217}
218
219inline double min3(double a, double b, double c)
220{
221    return min2(min2(a, b), c);
222}
223
224inline double max3(double a, double b, double c)
225{
226    return max2(max2(a, b), c);
227}
228
229}
230
231#endif
Note: See TracBrowser for help on using the repository browser.