mirror of
https://gitlab.kitware.com/vtk/vtk-m
synced 2024-09-20 11:05:44 +00:00
120 lines
3.1 KiB
C++
120 lines
3.1 KiB
C++
//=============================================================================
|
|
//
|
|
// Copyright (c) Kitware, Inc.
|
|
// All rights reserved.
|
|
// See LICENSE.txt for details.
|
|
//
|
|
// This software is distributed WITHOUT ANY WARRANTY; without even
|
|
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
|
|
// PURPOSE. See the above copyright notice for more information.
|
|
//
|
|
// Copyright 2015 Sandia Corporation.
|
|
// Copyright 2015 UT-Battelle, LLC.
|
|
// Copyright 2015 Los Alamos National Security.
|
|
//
|
|
// Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
|
|
// the U.S. Government retains certain rights in this software.
|
|
// Under the terms of Contract DE-AC52-06NA25396 with Los Alamos National
|
|
// Laboratory (LANL), the U.S. Government retains certain rights in
|
|
// this software.
|
|
//
|
|
//=============================================================================
|
|
|
|
/*
|
|
* quaternion.h
|
|
*
|
|
* Created on: Oct 10, 2014
|
|
* Author: csewell
|
|
*/
|
|
|
|
#ifndef QUATERNION_H_
|
|
#define QUATERNION_H_
|
|
|
|
#include <math.h>
|
|
#include <stdlib.h>
|
|
|
|
class Quaternion
|
|
{
|
|
public:
|
|
Quaternion()
|
|
{
|
|
x = y = z = 0.0;
|
|
w = 1.0;
|
|
}
|
|
Quaternion(double ax, double ay, double az, double aw)
|
|
: x(ax)
|
|
, y(ay)
|
|
, z(az)
|
|
, w(aw){};
|
|
void set(double ax, double ay, double az, double aw)
|
|
{
|
|
x = ax;
|
|
y = ay;
|
|
z = az;
|
|
w = aw;
|
|
}
|
|
void normalize()
|
|
{
|
|
float norm = static_cast<float>(sqrt(x * x + y * y + z * z + w * w));
|
|
if (norm > 0.00001)
|
|
{
|
|
x /= norm;
|
|
y /= norm;
|
|
z /= norm;
|
|
w /= norm;
|
|
}
|
|
}
|
|
void mul(Quaternion q)
|
|
{
|
|
double tx, ty, tz, tw;
|
|
tx = w * q.x + x * q.w + y * q.z - z * q.y;
|
|
ty = w * q.y + y * q.w + z * q.x - x * q.z;
|
|
tz = w * q.z + z * q.w + x * q.y - y * q.x;
|
|
tw = w * q.w - x * q.x - y * q.y - z * q.z;
|
|
|
|
x = tx;
|
|
y = ty;
|
|
z = tz;
|
|
w = tw;
|
|
}
|
|
void setEulerAngles(float pitch, float yaw, float roll)
|
|
{
|
|
w = cos(pitch / 2.0) * cos(yaw / 2.0) * cos(roll / 2.0) -
|
|
sin(pitch / 2.0) * sin(yaw / 2.0) * sin(roll / 2.0);
|
|
x = sin(pitch / 2.0) * sin(yaw / 2.0) * cos(roll / 2.0) +
|
|
cos(pitch / 2.0) * cos(yaw / 2.0) * sin(roll / 2.0);
|
|
y = sin(pitch / 2.0) * cos(yaw / 2.0) * cos(roll / 2.0) +
|
|
cos(pitch / 2.0) * sin(yaw / 2.0) * sin(roll / 2.0);
|
|
z = cos(pitch / 2.0) * sin(yaw / 2.0) * cos(roll / 2.0) -
|
|
sin(pitch / 2.0) * cos(yaw / 2.0) * sin(roll / 2.0);
|
|
|
|
normalize();
|
|
}
|
|
|
|
void getRotMat(float* m) const
|
|
{
|
|
for (int i = 0; i < 16; i++)
|
|
{
|
|
m[i] = 0.0;
|
|
}
|
|
|
|
m[0] = static_cast<float>(1.0 - 2.0 * y * y - 2.0 * z * z);
|
|
m[1] = static_cast<float>(2.0 * x * y - 2.0 * z * w);
|
|
m[2] = static_cast<float>(2.0 * x * z + 2.0 * y * w);
|
|
|
|
m[4] = static_cast<float>(2.0 * x * y + 2.0 * z * w);
|
|
m[5] = static_cast<float>(1.0 - 2.0 * x * x - 2.0 * z * z);
|
|
m[6] = static_cast<float>(2.0 * y * z - 2.0 * x * w);
|
|
|
|
m[8] = static_cast<float>(2.0 * x * z - 2.0 * y * w);
|
|
m[9] = static_cast<float>(2.0 * y * z + 2.0 * x * w);
|
|
m[10] = static_cast<float>(1.0 - 2.0 * x * x - 2.0 * y * y);
|
|
|
|
m[15] = 1.0;
|
|
}
|
|
|
|
double x, y, z, w;
|
|
};
|
|
|
|
#endif /* QUATERNION_H_ */
|