<commit_before>/* dtkMath.tpp --- 
 * 
 * Author: <NAME>
 * Copyright (C) 2008 - <NAME>, Inria.
 * Created: Wed Mar  9 11:05:40 2011 (+0100)
 * Version: $Id$
 * Last-Updated: Tue Apr 24 20:19:17 2012 (+0200)
 *           By: <NAME>
 *     Update #: 88
 */

/* Commentary: 
 * 
 */

/* Change log:
 * 
 */

#ifndef DTKMATH_TPP
#define DTKMATH_TPP

#include "dtkMatrixSquared.h"
#include "dtkQuaternion.h"
#include "dtkVector3D.h"

static double dtkDeg2Rad(double v)
{
    return v*M_PI/180.0;
}

static double dtkRad2Deg(double v)
{
    return v*180.0/M_PI;
}

template <class T> dtkVector3D<T> dtkRotate(const dtkQuaternion<T>& quaternion, const dtkVector3D<T>& vector)
{
    dtkQuaternion<T> q(quaternion*dtkQuaternion<T>(vector[0], vector[1], vector[2], 0)*dtkInverse<T>(quaternion));
    return dtkVector3D<T>(q[0], q[1], q[2]);
}

//! dtkMixedProduct(const dtkVector3D &v0, const dtkVector3D &v1, const dtkVector3D &v2)
/*! 
 * This function calculates the mixed product (also called triple scalar product)
 * formed by three dtkVector3D.
 */
template <class T>  T dtkMixedProduct(const dtkVector3D<T> &v0, 
                                      const dtkVector3D<T> &v1, 
                                      const dtkVector3D<T> &v2)
{
    dtkVector3D<T> v;
    v.storeOuterProduct(v1, v2);
    return (v0*v);
}

//! dtkQuaternionFromEulerAxisAndAngle(const dtkVector3D<T> &dtkVecEulerAxis, const T &rotateEulerAngle)
/*! 
 * This function returns a quaternion representing a rigid body's 
 * attitude from its Euler axis and angle. 
 * NOTE: Argument vector dtkVecEulerAxis MUST be a unit vector.
 */
template <class T> dtkQuaternion<T> dtkQuaternionFromAxisAngle(const dtkVector3D<T> &axis, const T &angle)
{
    dtkVector3D<T> v = axis.unit();

    return dtkQuaternion<T>(1.0, dtkVector3D<T>(axis[0], axis[1], axis[2]), dtkDeg2Rad(angle/(T)2));
}

template <class T> dtkQuaternion<T> dtkQuaternionFromHPR(T h, T p, T r)
{
    T  cosH, sinH, cosP, sinP, cosR, sinR;
    T  half_r, half_p, half_h;

   /* put angles into radians and divide by two, since all angles in formula
    *  are (angle/2)
    */

   half_h = dtkDeg2Rad(h / 2.0);
   half_p = dtkDeg2Rad(p / 2.0);
   half_r = dtkDeg2Rad(r / 2.0);

   cosH = cos(half_h);
   sinH = sin(half_h);

   cosP = cos(half_p);
   sinP = sin(half_p);

   cosR = cos(half_r);
   sinR = sin(half_r);

   return dtkQuaternion<T>(
        sinR * cosP * cosH - cosR * sinP * sinH,
        cosR * sinP * cosH + sinR * cosP * sinH,
        cosR * cosP * sinH - sinR * sinP * cosH,
        cosR * cosP * cosH + sinR * sinP * sinH);
}

//! dtkEulerAxisFromQuaternion(const dtkQuaternion<T> &qtn)
/*!
 * This function returns Euler axis from a quaternion representing a rigid body's
 * attitude.
 */
template <class T> dtkVector3D<T> dtkAxisFromQuaternion(const dtkQuaternion<T> &qtn)
{
    dtkVector3D<T> axis(qtn[0], qtn[1], qtn[2]);

    return axis.unit();
}

//! dtkQuaternionFromMatSquared(const dtkMatrixSquared<T> &mat)
/*! 
 * This function returns the quaternion corresponding to a
 * transformation matrix.
*/
template <class T> dtkQuaternion<T> dtkQuaternionFromMatSquared(const dtkMatrixSquared<T> &mat)
{
    T sclrTmp = dtkTrace(mat);

    dtkQuaternion<T> qtn;

    if(sclrTmp > 0) {
	sclrTmp = 0.5*sqrt(1 + sclrTmp);
	qtn[0] = 0.25*(mat[1][2] - mat[2][1])/sclrTmp;
	qtn[1] = 0.25*(mat[2][0] - mat[0][2])/sclrTmp;
	qtn[2] = 0.25*(mat[0][1] - mat[1][0])/sclrTmp;
	qtn[3] = sclrTmp;
    } else {
	// We could be in trouble, so we have to take
	// precautions. NOTE: Parts of this piece of code were taken
	// from the example in the article "Rotating Objects Using
	// Quaternions" in Game Developer Magazine written by Nick
	// Bobick, july 3, 1998, vol. 2, issue 26.
	int i = 0;
	if (mat[1][1] > mat[0][0]) i = 1;
	if (mat[2][2] > mat[i][i]) i = 2;

	switch (i) {
        case 0:
	    sclrTmp = 0.5*sqrt(1 + mat[0][0] - mat[1][1] - mat[2][2]);
	    qtn[0] = sclrTmp;
	    qtn[1] = 0.25*(mat[0][1] + mat[1][0])/sclrTmp;
	    qtn[2] = 0.25*(mat[0][2] + mat[2][0])/sclrTmp;
	    qtn[3] = 0.25*(mat[1][2] - mat[2][1])/sclrTmp;
	    break;<commit_msg>Fixing matrix trace call in math layer.<commit_after>/* dtkMath.tpp --- 
 * 
 * Author: <NAME>
 * Copyright (C) 2008 - <NAME>, Inria.
 * Created: Wed Mar  9 11:05:40 2011 (+0100)
 * Version: $Id$
 * Last-Updated: Tue Apr 24 21:10:02 2012 (+0200)
 *           By: <NAME>
 *     Update #: 89
 */

/* Commentary: 
 * 
 */

/* Change log:
 * 
 */

#ifndef DTKMATH_TPP
#define DTKMATH_TPP

#include "dtkMatrixSquared.h"
#include "dtkQuaternion.h"
#include "dtkVector3D.h"

static double dtkDeg2Rad(double v)
{
    return v*M_PI/180.0;
}

static double dtkRad2Deg(double v)
{
    return v*180.0/M_PI;
}

template <class T> dtkVector3D<T> dtkRotate(const dtkQuaternion<T>& quaternion, const dtkVector3D<T>& vector)
{
    dtkQuaternion<T> q(quaternion*dtkQuaternion<T>(vector[0], vector[1], vector[2], 0)*dtkInverse<T>(quaternion));
    return dtkVector3D<T>(q[0], q[1], q[2]);
}

//! dtkMixedProduct(const dtkVector3D &v0, const dtkVector3D &v1, const dtkVector3D &v2)
/*! 
 * This function calculates the mixed product (also called triple scalar product)
 * formed by three dtkVector3D.
 */
template <class T>  T dtkMixedProduct(const dtkVector3D<T> &v0, 
                                      const dtkVector3D<T> &v1, 
                                      const dtkVector3D<T> &v2)
{
    dtkVector3D<T> v;
    v.storeOuterProduct(v1, v2);
    return (v0*v);
}

//! dtkQuaternionFromEulerAxisAndAngle(const dtkVector3D<T> &dtkVecEulerAxis, const T &rotateEulerAngle)
/*! 
 * This function returns a quaternion representing a rigid body's 
 * attitude from its Euler axis and angle. 
 * NOTE: Argument vector dtkVecEulerAxis MUST be a unit vector.
 */
template <class T> dtkQuaternion<T> dtkQuaternionFromAxisAngle(const dtkVector3D<T> &axis, const T &angle)
{
    dtkVector3D<T> v = axis.unit();

    return dtkQuaternion<T>(1.0, dtkVector3D<T>(axis[0], axis[1], axis[2]), dtkDeg2Rad(angle/(T)2));
}

template <class T> dtkQuaternion<T> dtkQuaternionFromHPR(T h, T p, T r)
{
    T  cosH, sinH, cosP, sinP, cosR, sinR;
    T  half_r, half_p, half_h;

   /* put angles into radians and divide by two, since all angles in formula
    *  are (angle/2)
    */

   half_h = dtkDeg2Rad(h / 2.0);
   half_p = dtkDeg2Rad(p / 2.0);
   half_r = dtkDeg2Rad(r / 2.0);

   cosH = cos(half_h);
   sinH = sin(half_h);

   cosP = cos(half_p);
   sinP = sin(half_p);

   cosR = cos(half_r);
   sinR = sin(half_r);

   return dtkQuaternion<T>(
        sinR * cosP * cosH - cosR * sinP * sinH,
        cosR * sinP * cosH + sinR * cosP * sinH,
        cosR * cosP * sinH - sinR * sinP * cosH,
        cosR * cosP * cosH + sinR * sinP * sinH);
}

//! dtkEulerAxisFromQuaternion(const dtkQuaternion<T> &qtn)
/*!
 * This function returns Euler axis from a quaternion representing a rigid body's
 * attitude.
 */
template <class T> dtkVector3D<T> dtkAxisFromQuaternion(const dtkQuaternion<T> &qtn)
{
    dtkVector3D<T> axis(qtn[0], qtn[1], qtn[2]);

    return axis.unit();
}

//! dtkQuaternionFromMatSquared(const dtkMatrixSquared<T> &mat)
/*! 
 * This function returns the quaternion corresponding to a
 * transformation matrix.
*/
template <class T> dtkQuaternion<T> dtkQuaternionFromMatSquared(const dtkMatrixSquared<T> &mat)
{
    T sclrTmp = dtkMatrixSquaredTrace(mat);

    dtkQuaternion<T> qtn;

    if(sclrTmp > 0) {
	sclrTmp = 0.5*sqrt(1 + sclrTmp);
	qtn[0] = 0.25*(mat[1][2] - mat[2][1])/sclrTmp;
	qtn[1] = 0.25*(mat[2][0] - mat[0][2])/sclrTmp;
	qtn[2] = 0.25*(mat[0][1] - mat[1][0])/sclrTmp;
	qtn[3] = sclrTmp;
    } else {
	// We could be in trouble, so we have to take
	// precautions. NOTE: Parts of this piece of code were taken
	// from the example in the article "Rotating Objects Using
	// Quaternions" in Game Developer Magazine written by Nick
	// Bobick, july 3, 1998, vol. 2, issue 26.
	int i = 0;
	if (mat[1][1] > mat[0][0]) i = 1;
	if (mat[2][2] > mat[i][i]) i = 2;

	switch (i) {
        case 0:
	    sclrTmp = 0.5*sqrt(1 + mat[0][0] - mat[1][1] - mat[2][2]);
	    qtn[0] = sclrTmp;
	    qtn[1] = 0.25*(mat[0][1] + mat[1][0])/sclrTmp;
	    qtn[2] = 0.25*(mat[0][2] + mat[2][0])/sclrTmp;
	    qtn[3] = 0.25*(mat[1][2] - mat[2][1])/sclrTmp;
	    break;
{'fraction_non_alphanumeric': 0.15517867958812842, 'fraction_numerical': 0.04409448818897638, 'mean_word_length': 3.2732919254658386, 'pattern_counts': {'":': 0, '<': 75, '<?xml version=': 0, '>': 81, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5595333', 'n_tokens_mistral': 3672, 'n_tokens_neox': 3411, 'n_words': 803}