mirror of
https://gitlab.kitware.com/vtk/vtk-m
synced 2024-09-19 10:35:42 +00:00
91 lines
2.8 KiB
C++
Executable File
91 lines
2.8 KiB
C++
Executable File
//=============================================================================
|
|
//
|
|
// 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_ */
|