add btFileUtils::toLower to convert a string (char*) to lower case
URDF import demo: add COLLADA .dae file support add FiniteElementMethod demo, extracted from the OpenTissue library (under the zlib license) don't crash if loading an invalid STL file add comparison with Assimp for COLLADA file loading (disabled by default, to avoid library dependency) Gwen: disable some flags that make the build incompatible
This commit is contained in:
erwin coumans
@@ -0,0 +1,52 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_BASIC_TYPES_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_BASIC_TYPES_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <OpenTissue/core/math/math_vector3.h>
|
||||
#include <OpenTissue/core/math/math_matrix3x3.h>
|
||||
#include <OpenTissue/core/math/math_quaternion.h>
|
||||
#include <OpenTissue/core/math/math_dual_quaternion.h>
|
||||
#include <OpenTissue/core/math/math_coordsys.h>
|
||||
#include <OpenTissue/core/math/math_value_traits.h>
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
namespace math
|
||||
{
|
||||
|
||||
// TODO add doxygen documentation explaining rationale behind this typebinder class. Why have we made it, and for what purpose? Also give example usages
|
||||
template< typename real_type_
|
||||
, typename index_type_
|
||||
>
|
||||
class BasicMathTypes
|
||||
{
|
||||
public:
|
||||
|
||||
typedef real_type_ real_type;
|
||||
typedef index_type_ index_type;
|
||||
typedef Vector3<real_type> vector3_type;
|
||||
typedef Quaternion<real_type> quaternion_type;
|
||||
typedef DualQuaternion<real_type> dual_quaternion_type;
|
||||
typedef Matrix3x3<real_type> matrix3x3_type;
|
||||
typedef CoordSys<real_type> coordsys_type;
|
||||
|
||||
typedef Vector3<index_type> index_vector3_type;
|
||||
typedef ValueTraits<real_type> value_traits;
|
||||
};
|
||||
|
||||
|
||||
// TODO add doxygen comments explaining what this type is useful for
|
||||
typedef BasicMathTypes<double,unsigned int> default_math_types;
|
||||
|
||||
} // namespace math
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_MATH_BASIC_TYPES_H
|
||||
#endif
|
||||
@@ -0,0 +1,139 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_COMPUTE_CONTIGUOUS_ANGLE_INTERVAL_H
|
||||
#define OPENTISSUE_CORE_MATH_COMPUTE_CONTIGUOUS_ANGLE_INTERVAL_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <OpenTissue/core/math/math_value_traits.h>
|
||||
#include <OpenTissue/core/math/math_is_number.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
#include <stdexcept>
|
||||
#include <cmath>
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
namespace math
|
||||
{
|
||||
|
||||
/**
|
||||
* Compute Contiguous Angle Interval.
|
||||
* This function tries to find a contiguous interval of angle values. As
|
||||
* input it takes a sequence of angle values. From this sequence the
|
||||
* function tries to compute the starting and ending angle (in terms
|
||||
* of counter-clock-wise rotation order) of a contiguous interval.
|
||||
* interval containing all the angles.
|
||||
*
|
||||
* One could interact with the function as follows
|
||||
*
|
||||
* std::vector<double> angles;
|
||||
*
|
||||
* ... fill angles with values ...
|
||||
*
|
||||
* double min_val, max_val;
|
||||
* compute_contiguous_angle_interval( angles.begin(), angles.end(), min_val, max_val );
|
||||
*
|
||||
* The resulting minimum and maximum values yields the starting and ending points
|
||||
* of the contiguous interval. The periodicity of rotation angles is used to adjust
|
||||
* the end-points of the interval to always be positive values.
|
||||
*
|
||||
* The supplied angles can be given in any sort of interval as long as
|
||||
* the width of the interval is no longer than 2 pi ( one full rotation). Typically
|
||||
* one would supply values from either -pi..pi or 0..2pi intervals.
|
||||
*
|
||||
* @param begin An iterator to the first angle value
|
||||
* @param end An iterator to one past the last angle value
|
||||
* @param theta_min Upon return this value holds the starting value of the contiguous angle interval.
|
||||
* @param theta_max Upon return this value holds the ending value of the contiguous angle interval.
|
||||
*/
|
||||
template<typename iterator_type>
|
||||
inline void compute_contiguous_angle_interval(
|
||||
iterator_type const & begin
|
||||
, iterator_type const & end
|
||||
, typename iterator_type::value_type & theta_min
|
||||
, typename iterator_type::value_type & theta_max )
|
||||
{
|
||||
//using std::cos;
|
||||
//using std::sin;
|
||||
//using std::sqrt;
|
||||
//using std::atan2;
|
||||
|
||||
typedef typename iterator_type::value_type T;
|
||||
typedef OpenTissue::math::ValueTraits<T> value_traits;
|
||||
|
||||
|
||||
T const two_pi = value_traits::pi()*value_traits::two();
|
||||
|
||||
// Determine the number of samples
|
||||
size_t const N = std::distance( begin, end);
|
||||
|
||||
// Make a copy of the samples and sort them in ascending order
|
||||
std::vector<T> storage;
|
||||
storage.resize( N );
|
||||
std::copy( begin, end, storage.begin() );
|
||||
std::sort( storage.begin(), storage.end() );
|
||||
|
||||
// Find the two theta values with the largest gap inbetween. That
|
||||
// is the largest angle difference as measured in a counter-clock-wise manner.
|
||||
T max_delta_theta = value_traits::zero();
|
||||
size_t max_i = N;
|
||||
|
||||
for(size_t i = 0u;i<N;++i)
|
||||
{
|
||||
T const & theta_i = storage[i];
|
||||
T const & theta_j = storage[(i+1)%N];
|
||||
T const delta_theta = (theta_j < theta_i) ? theta_j+two_pi-theta_i : theta_j-theta_i;
|
||||
if(delta_theta > max_delta_theta)
|
||||
{
|
||||
max_i = i;
|
||||
max_delta_theta = delta_theta;
|
||||
theta_min = theta_j;
|
||||
theta_max = theta_i;
|
||||
}
|
||||
}
|
||||
|
||||
//// Compute the mean angle by converting angles to points on
|
||||
//// the unit-circle and calculating the mean point.
|
||||
//T mx = value_traits::zero();
|
||||
//T my = value_traits::zero();
|
||||
//for(iterator_type theta = begin; theta != end; ++ theta)
|
||||
//{
|
||||
// T const c = cos( *theta );
|
||||
// T const s = sin( *theta );
|
||||
// mx += c;
|
||||
// my += s;
|
||||
//}
|
||||
|
||||
//T norm = sqrt( mx*mx + my*my);
|
||||
//assert( is_number(norm) || !"compute_contiguous_angle_interval(): NaN encountered");
|
||||
|
||||
//if(norm<= value_traits::zero())
|
||||
// throw std::logic_error("insufficient angle samples to determine a mean angle");
|
||||
|
||||
//mx /= norm;
|
||||
//my /= norm;
|
||||
|
||||
//assert( is_number(mx) || !"compute_contiguous_angle_interval(): NaN encountered");
|
||||
//assert( is_number(my) || !"compute_contiguous_angle_interval(): NaN encountered");
|
||||
|
||||
while(theta_min < value_traits::zero())
|
||||
theta_min += two_pi;
|
||||
|
||||
while(theta_max < theta_min)
|
||||
theta_max += two_pi;
|
||||
|
||||
assert( theta_min <= theta_max || !"compute_contiguous_angle_interval(): invalid interval");
|
||||
}
|
||||
|
||||
|
||||
} // namespace math
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_COMPUTE_CONTIGUOUS_ANGLE_INTERVAL_H
|
||||
#endif
|
||||
209
Demos3/FiniteElementMethod/OpenTissue/core/math/math_constants.h
Normal file
209
Demos3/FiniteElementMethod/OpenTissue/core/math/math_constants.h
Normal file
@@ -0,0 +1,209 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_CONSTANTS_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_CONSTANTS_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
//#include <boost/numeric/conversion/bounds.hpp>
|
||||
#include <cmath>
|
||||
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
// constant: 0 (zero)
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
inline T zero();
|
||||
|
||||
template <>
|
||||
inline float zero<float>() { return 0.0f; }
|
||||
|
||||
template <>
|
||||
inline double zero<double>() { return 0.0; }
|
||||
|
||||
template <>
|
||||
inline int zero<int>() { return 0; }
|
||||
|
||||
template <>
|
||||
inline long unsigned int zero<long unsigned int>() { return 0; }
|
||||
|
||||
template <>
|
||||
inline unsigned int zero<unsigned int>() { return 0u; }
|
||||
|
||||
} // namespace detail
|
||||
|
||||
|
||||
// constant: 1 (one)
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
inline T one();
|
||||
|
||||
template <>
|
||||
inline float one<float>() { return 1.0f; }
|
||||
|
||||
template <>
|
||||
inline double one<double>() { return 1.0; }
|
||||
|
||||
template <>
|
||||
inline int one<int>() { return 1; }
|
||||
|
||||
template <>
|
||||
inline unsigned int one<unsigned int>() { return 1u; }
|
||||
|
||||
} // namespace detail
|
||||
|
||||
|
||||
// constant: 2 (two)
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
inline T two();
|
||||
|
||||
template <>
|
||||
inline float two<float>() { return 2.0f; }
|
||||
|
||||
template <>
|
||||
inline double two<double>() { return 2.0; }
|
||||
|
||||
template <>
|
||||
inline int two<int>() { return 2; }
|
||||
|
||||
template <>
|
||||
inline unsigned int two<unsigned int>() { return 2u; }
|
||||
|
||||
} // namespace detail
|
||||
|
||||
|
||||
// constant: 3 (three)
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
inline T three();
|
||||
|
||||
template <>
|
||||
inline float three<float>() { return 3.0f; }
|
||||
|
||||
template <>
|
||||
inline double three<double>() { return 3.0; }
|
||||
|
||||
template <>
|
||||
inline int three<int>() { return 3; }
|
||||
|
||||
template <>
|
||||
inline unsigned int three<unsigned int>() { return 3u; }
|
||||
|
||||
} // namespace detail
|
||||
|
||||
|
||||
// constant: 4 (four)
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
inline T four();
|
||||
|
||||
template <>
|
||||
inline float four<float>() { return 4.0f; }
|
||||
|
||||
template <>
|
||||
inline double four<double>() { return 4.0; }
|
||||
|
||||
template <>
|
||||
inline int four<int>() { return 4; }
|
||||
|
||||
template <>
|
||||
inline unsigned int four<unsigned int>() { return 4u; }
|
||||
|
||||
} // namespace detail
|
||||
|
||||
|
||||
// constant: 8 (eight)
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
inline T eight();
|
||||
|
||||
template <>
|
||||
inline float eight<float>() { return 8.0f; }
|
||||
|
||||
template <>
|
||||
inline double eight<double>() { return 8.0; }
|
||||
|
||||
template <>
|
||||
inline int eight<int>() { return 8; }
|
||||
|
||||
template <>
|
||||
inline unsigned int eight<unsigned int>() { return 8u; }
|
||||
|
||||
} // namespace detail
|
||||
|
||||
|
||||
// constant: <20> (half)
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
inline T half();
|
||||
|
||||
template <>
|
||||
inline float half<float>() { return 0.5f; }
|
||||
|
||||
template <>
|
||||
inline double half<double>() { return 0.5; }
|
||||
|
||||
} // namespace detail
|
||||
|
||||
|
||||
// constant: pi and fractions of pi
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template<typename T>
|
||||
inline T pi() { return (T)(M_PI); }
|
||||
|
||||
template<typename T>
|
||||
inline T pi_half() { return (T)(M_PI_2); }
|
||||
|
||||
template<typename T>
|
||||
inline T pi_quarter() { return (T)(M_PI_4); }
|
||||
|
||||
} // namespace detail
|
||||
|
||||
|
||||
// constant: specials
|
||||
namespace detail
|
||||
{
|
||||
|
||||
|
||||
// one degree in radians
|
||||
template<typename T>
|
||||
inline T degree() { return (T)(0.017453292519943295769236907684886); }
|
||||
|
||||
// one radian in degrees
|
||||
template<typename T>
|
||||
inline T radian() { return (T)(57.295779513082320876798154814105); }
|
||||
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
// #define OPENTISSUE_CORE_MATH_MATH_CONSTANTS_H
|
||||
#endif
|
||||
320
Demos3/FiniteElementMethod/OpenTissue/core/math/math_coordsys.h
Normal file
320
Demos3/FiniteElementMethod/OpenTissue/core/math/math_coordsys.h
Normal file
@@ -0,0 +1,320 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_COORDSYS_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_COORDSYS_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <OpenTissue/core/math/math_vector3.h>
|
||||
#include <OpenTissue/core/math/math_matrix3x3.h>
|
||||
#include <OpenTissue/core/math/math_quaternion.h>
|
||||
#include <OpenTissue/core/math/math_value_traits.h>
|
||||
|
||||
#include <iosfwd>
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
/**
|
||||
* A Coordinate System.
|
||||
*
|
||||
* Remeber, this represents a transform that brings you from a local frame
|
||||
* into a global frame, that is:
|
||||
*
|
||||
* BF -> WCS
|
||||
*/
|
||||
template<
|
||||
typename value_type_
|
||||
//, typename value_traits_ = ValueTraits<value_type_>
|
||||
>
|
||||
class CoordSys
|
||||
{
|
||||
protected:
|
||||
|
||||
typedef typename OpenTissue::math::ValueTraits<value_type_> value_traits_ ; // TODO value_traits_ should be parameterized as a class template parameter.
|
||||
|
||||
public:
|
||||
|
||||
typedef value_traits_ value_traits; ///< Convenience typedef to make value traits accessible for all template functions using Vector3 types.
|
||||
typedef value_type_ value_type; ///< Typedef is required for compliance with many other libraries and data containers!
|
||||
|
||||
typedef Vector3<value_type> vector3_type;
|
||||
typedef Quaternion<value_type> quaternion_type;
|
||||
typedef Matrix3x3<value_type> matrix3x3_type;
|
||||
typedef Quaternion<value_type> rotation_type; ///< typedef to make interface for different coordsystypes the same
|
||||
|
||||
protected:
|
||||
|
||||
vector3_type m_T; ///< The Position.
|
||||
quaternion_type m_Q; ///< The orientation in Quaternion form.
|
||||
|
||||
public:
|
||||
|
||||
vector3_type & T() { return m_T; }
|
||||
vector3_type const & T() const { return m_T; }
|
||||
quaternion_type & Q() { return m_Q; }
|
||||
quaternion_type const & Q() const { return m_Q; }
|
||||
|
||||
public:
|
||||
|
||||
CoordSys()
|
||||
: m_T(value_traits::zero(),value_traits::zero(),value_traits::zero())
|
||||
, m_Q(value_traits::one(),value_traits::zero(),value_traits::zero(),value_traits::zero())
|
||||
{}
|
||||
|
||||
explicit CoordSys(vector3_type const & T_val, quaternion_type const & Q_val)
|
||||
{
|
||||
m_T = T_val;
|
||||
m_Q = unit(Q_val);
|
||||
}
|
||||
|
||||
explicit CoordSys(vector3_type const & T_val, matrix3x3_type const & R_val)
|
||||
{
|
||||
m_T = T_val;
|
||||
m_Q = R_val;
|
||||
}
|
||||
|
||||
CoordSys & operator=(CoordSys const & C)
|
||||
{
|
||||
m_T = C.m_T;
|
||||
m_Q = C.m_Q;
|
||||
return *this;
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
// TODO: Comparing floats with == or != is not safe NOTE T might not be a float type it could be anything? This suggest that we need some kind of metaprogramming technique to deal with this problem?
|
||||
bool operator==(CoordSys const & C) const { return m_T == C.m_T && m_Q==C.m_Q; }
|
||||
|
||||
public:
|
||||
|
||||
void identity()
|
||||
{
|
||||
m_T.clear();
|
||||
m_Q.identity();
|
||||
}
|
||||
|
||||
/**
|
||||
* This method assumes that the point is in this coordinate system.
|
||||
* In other words this method maps local points into non local
|
||||
* points:
|
||||
*
|
||||
* BF -> WCS
|
||||
*
|
||||
* Let p be the point and let f designate the function of this
|
||||
* method then we have
|
||||
*
|
||||
* [p]_WCS = f(p)
|
||||
*
|
||||
*/
|
||||
void xform_point(vector3_type & p) const { p = m_Q.rotate(p) + m_T; }
|
||||
|
||||
/**
|
||||
* This method assumes that the vector is in this
|
||||
* coordinate system. That is it maps the vector
|
||||
* from BF into WCS.
|
||||
*/
|
||||
void xform_vector(vector3_type & v) const { v = m_Q.rotate(v); }
|
||||
|
||||
/**
|
||||
* Transform Matrix.
|
||||
*
|
||||
* @param O A reference to a rotation matrix, which should be transformed.
|
||||
*/
|
||||
void xform_matrix(matrix3x3_type & O) const
|
||||
{
|
||||
O = matrix3x3_type(m_Q) * O;
|
||||
}
|
||||
|
||||
/**
|
||||
* Transform Coordinate System.
|
||||
* This method transforms the specified coordinate
|
||||
* system by this coordinate transform.
|
||||
*
|
||||
* That is:
|
||||
*
|
||||
* Tnew = Tx Rx Told
|
||||
* Rnew = Rx Rold
|
||||
*
|
||||
* @param X The transform by which the current
|
||||
* transform should be changed with..
|
||||
*/
|
||||
CoordSys operator*(CoordSys const & X1) const
|
||||
{
|
||||
return CoordSys( m_Q.rotate(X1.T()) + m_T , unit( prod( m_Q , X1.Q()) ) );
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
bool is_equal(CoordSys const & C, value_type const & accuracy) const
|
||||
{
|
||||
return m_T.is_equal(C.m_T,accuracy) && m_Q.is_equal(C.m_Q,accuracy);
|
||||
}
|
||||
|
||||
}; // class CoordSys
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Coordinate Transformation Product.
|
||||
* This function should be used to concatenate coordinate transformations.
|
||||
* In terms of homogeneous coordinates L and R corresponds to the matrices.
|
||||
*
|
||||
* L = | R_l T_l |
|
||||
* | 0 1 |
|
||||
*
|
||||
* R = | R_r T_r |
|
||||
* | 0 1 |
|
||||
*
|
||||
* This function computes the equivalent of the product
|
||||
*
|
||||
* X = L R
|
||||
*
|
||||
* = | R_l T_l | | R_r T_r |
|
||||
* | 0 1 | | 0 1 |
|
||||
*
|
||||
* = | R_l R_r R_l T_r + T_l |
|
||||
* | 0 1 |
|
||||
*
|
||||
* @param L The left coordinate transformation
|
||||
* @param R The right coordinate transformation
|
||||
*
|
||||
* @return The coordinate transformation corresponding to the product L*R.
|
||||
*/
|
||||
template<typename T>
|
||||
inline CoordSys<T> prod(CoordSys<T> const & L, CoordSys<T> const & R)
|
||||
{
|
||||
return CoordSys<T>( L.Q().rotate( R.T() ) + L.T() , unit( prod( L.Q() , R.Q()) ) );
|
||||
}
|
||||
|
||||
/**
|
||||
* Inverse Transform.
|
||||
*
|
||||
* If we have
|
||||
*
|
||||
* BF -> WCS
|
||||
*
|
||||
* Then we want to find
|
||||
*
|
||||
* WCS -> BF
|
||||
*
|
||||
*/
|
||||
template<typename T>
|
||||
inline CoordSys<T> inverse(CoordSys<T> const & X)
|
||||
{
|
||||
//---
|
||||
//--- p' = R p + T
|
||||
//---
|
||||
//--- =>
|
||||
//---
|
||||
//--- p = R^{-1} p' + R^{-1}(-T)
|
||||
//---
|
||||
return CoordSys<T>( conj(X.Q()).rotate(-X.T()) , conj(X.Q()) );
|
||||
}
|
||||
|
||||
/**
|
||||
* Model Update Transform.
|
||||
* This method computes the necessary transform needed in
|
||||
* order to transform coordinates from one local frame
|
||||
* into another local frame. This utility is useful when
|
||||
* one wants to do model updates instead of world updates.
|
||||
*
|
||||
* In mathematical terms we have two transforms:
|
||||
*
|
||||
* C1 : H -> G
|
||||
* C2 : F -> G
|
||||
*
|
||||
* And we want to find
|
||||
*
|
||||
* C3 : H -> F
|
||||
*
|
||||
* This transform is computed and assigned to this coordinate
|
||||
* system.
|
||||
*/
|
||||
template<typename T>
|
||||
inline CoordSys<T> model_update(CoordSys<T> const & A, CoordSys<T> const & B)
|
||||
{
|
||||
return model_update(A.T(),A.Q(),B.T(),B.Q());
|
||||
}
|
||||
|
||||
/**
|
||||
* Model Update Transform.
|
||||
* This method computes the necessary transform needed in
|
||||
* order to transform coordinates from one local frame
|
||||
* into another local frame. This utility is useful when
|
||||
* one wants to do model updates instead of world updates.
|
||||
*
|
||||
* In mathematical terms we have two transforms:
|
||||
*
|
||||
* C1 : H -> G
|
||||
* C2 : F -> G
|
||||
*
|
||||
* And we want to find
|
||||
*
|
||||
* C3 : H -> F
|
||||
*
|
||||
* This transform is computed and assigned to this coordinate
|
||||
* system.
|
||||
*
|
||||
* Very important: Note that this method finds the transform A -> B.
|
||||
*/
|
||||
template<typename T>
|
||||
inline CoordSys<T> model_update(Vector3<T> const & TA, Quaternion<T> const & QA, Vector3<T> const & TB, Quaternion<T> const & QB)
|
||||
{
|
||||
//---
|
||||
//--- p' = RA p + TA (*1) from A->WCS
|
||||
//---
|
||||
//--- p = RB^T (p' - TB) (*2) from WCS-B
|
||||
//---
|
||||
//--- Insert (*1) into (*2) A -> B
|
||||
//---
|
||||
//--- p = RB^T ( RA p + TA - TB)
|
||||
//--- = RB^T RA p + RB^T (TA - TB)
|
||||
//--- So
|
||||
//--- R = RB^T RA
|
||||
//--- T = RB^T (TA - TB)
|
||||
//---
|
||||
Quaternion<T> q;
|
||||
if(QA==QB)
|
||||
{
|
||||
q.identity();
|
||||
}
|
||||
else
|
||||
{
|
||||
q = unit( prod( conj(QB), QA) );
|
||||
}
|
||||
return CoordSys<T>( conj(QB).rotate(TA - TB), q);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline std::ostream & operator<< (std::ostream & o, CoordSys<T> const & C)
|
||||
{
|
||||
o << "[" << C.T() << "," << C.Q() << "]";
|
||||
return o;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline std::istream & operator>>(std::istream & i, CoordSys<T> & C)
|
||||
{
|
||||
char dummy;
|
||||
i >> dummy;
|
||||
i >> C.T();
|
||||
i >> dummy;
|
||||
i >> C.Q();
|
||||
i >> dummy;
|
||||
return i;
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_MATH_COORDSYS_H
|
||||
#endif
|
||||
@@ -0,0 +1,84 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_COVARIANCE_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_COVARIANCE_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
/**
|
||||
*
|
||||
* @param mean Resulting mean point.
|
||||
* @param C Resulting covariance matrix.
|
||||
*/
|
||||
template< typename vector3_iterator, typename vector3_type, typename matrix3x3_type>
|
||||
inline void
|
||||
covariance(
|
||||
vector3_iterator begin, vector3_iterator end
|
||||
, vector3_type & mean, matrix3x3_type & C
|
||||
)
|
||||
{
|
||||
typedef typename vector3_type::value_traits value_traits;
|
||||
|
||||
unsigned int N = 0;
|
||||
|
||||
mean.clear();
|
||||
|
||||
for(vector3_iterator v = begin;v!=end;++v,++N)
|
||||
mean += (*v);
|
||||
mean /= N;
|
||||
|
||||
C.clear();
|
||||
for(vector3_iterator v = begin;v!=end;++v,++N)
|
||||
{
|
||||
C(0,0) += ((*v)(0) - mean(0))*((*v)(0) - mean(0));
|
||||
C(1,1) += ((*v)(1) - mean(1))*((*v)(1) - mean(1));
|
||||
C(2,2) += ((*v)(2) - mean(2))*((*v)(2) - mean(2));
|
||||
C(0,1) += ((*v)(0) - mean(0))*((*v)(1) - mean(1));
|
||||
C(0,2) += ((*v)(0) - mean(0))*((*v)(2) - mean(2));
|
||||
C(1,2) += ((*v)(1) - mean(1))*((*v)(2) - mean(2));
|
||||
}
|
||||
C(1,0) = C(0,1);
|
||||
C(2,0) = C(0,2);
|
||||
C(2,1) = C(1,2);
|
||||
C /= N;
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
*
|
||||
* @param mean Resulting mean point.
|
||||
* @param C Resulting covariance matrix.
|
||||
*/
|
||||
template<typename vector3_type, typename matrix3x3_type>
|
||||
inline void
|
||||
covariance_union(
|
||||
vector3_type const & mean1, matrix3x3_type const & C1
|
||||
, vector3_type const & mean2, matrix3x3_type const & C2
|
||||
, vector3_type & mean, matrix3x3_type & C
|
||||
)
|
||||
{
|
||||
typedef typename vector3_type::value_traits value_traits;
|
||||
|
||||
mean = (mean1 + mean2)/value_traits::two();
|
||||
matrix3x3_type KK = outer_prod<matrix3x3_type,vector3_type>(mean,mean);
|
||||
matrix3x3_type NN = outer_prod<matrix3x3_type,vector3_type>(mean1,mean1);
|
||||
matrix3x3_type MM = outer_prod<matrix3x3_type,vector3_type>(mean2,mean2);
|
||||
C = ((C1 + NN + C2 + MM)/value_traits::two() - KK) ;
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
// OPENTISSUE_CORE_MATH_MATH_COVARIANCE_H
|
||||
#endif
|
||||
@@ -0,0 +1,198 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_DUAL_QUATERNION_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_DUAL_QUATERNION_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2010 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <OpenTissue/core/math/math_value_traits.h>
|
||||
|
||||
#include <OpenTissue/core/math/math_quaternion.h>
|
||||
|
||||
#include <iosfwd>
|
||||
|
||||
// 2010-07-16 Kenny : code review, where is the unit tests?
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
// 2010-07-16 Kenny: code review, why do you keep uncommented code around?
|
||||
// 2010-07-16 perb: I have keept this as I thought this was the direction OpenTissue was going,
|
||||
// considering that it is in both the vector3 and quaternion class
|
||||
// I thought it was a preparation to get rid of the next TODO...
|
||||
template <
|
||||
typename value_type_
|
||||
//, typename value_traits_ = ValueTraits<value_type_>
|
||||
>
|
||||
class DualQuaternion
|
||||
{
|
||||
protected:
|
||||
|
||||
// 2010-07-16 Kenny: code review, the TODO comment seems unneeded?
|
||||
// 2010-07-16 perb
|
||||
typedef typename OpenTissue::math::ValueTraits<value_type_> value_traits_ ; // TODO value_traits_ should be parameterized as a class template parameter.
|
||||
|
||||
public:
|
||||
|
||||
typedef value_traits_ value_traits; ///< Convience typedef to make value traits accesible for all template functions using Vector3 types.
|
||||
typedef value_type_ value_type; ///< Typedef is required for compliance with many other libraries and data containers!
|
||||
typedef size_t index_type;
|
||||
typedef Vector3<value_type> vector3_type;
|
||||
typedef Quaternion<value_type> quaternion_type;
|
||||
|
||||
protected:
|
||||
|
||||
quaternion_type m_real; ///< The real part of this dual quaternion. This represents the rotation when used as a rigid motion.
|
||||
quaternion_type m_dual; ///< The dual part of this dual quaternion. This represents the displacement when used as a rigid motion.
|
||||
|
||||
public:
|
||||
|
||||
quaternion_type & r() { return m_real; }
|
||||
quaternion_type const & r() const { return m_real; }
|
||||
|
||||
quaternion_type & d() { return m_dual; }
|
||||
quaternion_type const & d() const { return m_dual; }
|
||||
|
||||
public:
|
||||
|
||||
DualQuaternion()
|
||||
: m_real( quaternion_type( value_traits::one(), value_traits::zero(), value_traits::zero(), value_traits::zero() ) )
|
||||
, m_dual( quaternion_type( value_traits::zero(), value_traits::zero(), value_traits::zero(), value_traits::zero() ) )
|
||||
{}
|
||||
|
||||
/**
|
||||
* This constructs a dual quaternion representing the transformation of the given vector
|
||||
*
|
||||
* @param v Vector describing a translation
|
||||
*
|
||||
* @return A dual quaternion describing the translation given
|
||||
*/
|
||||
DualQuaternion( vector3_type const & v )
|
||||
: m_real( quaternion_type( value_traits::one(), value_traits::zero(), value_traits::zero(), value_traits::zero() ) )
|
||||
, m_dual( quaternion_type( value_traits::one(), v[0] / value_traits::two(), v[1] / value_traits::two(), v[2] / value_traits::two() ) )
|
||||
{}
|
||||
|
||||
/**
|
||||
* This constructs a unit dual quaternion describing a rotation and a translation
|
||||
*
|
||||
* @param rotation A unit quaternion describing a rotation
|
||||
* @param translation A vector describing a translation
|
||||
*
|
||||
* @return A unit dual quaternion describing the rotation and translation given
|
||||
*/
|
||||
DualQuaternion( quaternion_type const & rotation, vector3_type const & translation )
|
||||
: m_real( rotation )
|
||||
, m_dual( prod( quaternion_type( value_traits::zero(), translation[0], translation[1], translation[2] ), rotation) / value_traits::two() )
|
||||
{}
|
||||
|
||||
DualQuaternion( DualQuaternion< value_type > const & d ) { *this = d; }
|
||||
|
||||
DualQuaternion( quaternion_type const & s_val, quaternion_type const & d_val )
|
||||
: m_real( s_val )
|
||||
, m_dual( d_val )
|
||||
{}
|
||||
|
||||
~DualQuaternion()
|
||||
{}
|
||||
|
||||
DualQuaternion & operator=( DualQuaternion const & cpy )
|
||||
{
|
||||
m_real = cpy.m_real;
|
||||
m_dual = cpy.m_dual;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
DualQuaternion & operator+= ( DualQuaternion const & qd )
|
||||
{
|
||||
// 2010-07-16 Kenny: code review, safe guard for self assignment? if (this!=&qd)???
|
||||
// 2010-07-16 perb: I am not sure why this would be a problem.
|
||||
// 2010-07-16 kenny: Argh, sorry, I read the code too fast, I read it as an assignment operator.
|
||||
m_real += qd.r();
|
||||
m_dual += qd.d();
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
friend std::ostream & operator<< ( std::ostream & o, DualQuaternion< value_type > const & d )
|
||||
{
|
||||
o << "[" << d.r() << "," << d.d() << "]";
|
||||
|
||||
return o;
|
||||
}
|
||||
|
||||
friend std::istream & operator>>( std::istream & i, DualQuaternion< value_type > & d )
|
||||
{
|
||||
char dummy;
|
||||
|
||||
i >> dummy;
|
||||
i >> d.r();
|
||||
i >> dummy;
|
||||
i >> d.d();
|
||||
i >> dummy;
|
||||
|
||||
return i;
|
||||
}
|
||||
}; // class DualQuaternion
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
/// Declaration of DualQuaternion non-member functions
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
template < typename T, typename T2 >
|
||||
inline DualQuaternion<T> operator*( DualQuaternion<T> const& dq, T2 const& v ) { return DualQuaternion< T >( dq.r() * v, dq.d() * v ); }
|
||||
|
||||
template < typename T2, typename T >
|
||||
inline DualQuaternion<T> operator*( T2 const& v, DualQuaternion<T> const& dq ) { return DualQuaternion< T >( dq.r() * v, dq.d() * v ); }
|
||||
|
||||
template < typename T, typename T2 >
|
||||
inline DualQuaternion<T> operator/( DualQuaternion<T> const& dq, T2 const& v ) { return DualQuaternion< T >( dq.r() / v, dq.d() / v ); }
|
||||
|
||||
template < typename T >
|
||||
inline DualQuaternion< T > operator-( DualQuaternion< T > q ) { return DualQuaternion< T >( -q.r(), -q.d() ); }
|
||||
|
||||
|
||||
/**
|
||||
* Dual conjugate. This is the conjugate that considers the dual quaternion as a dual number with quaternion elements.
|
||||
*
|
||||
* @warning There are two different types. The quaternion conjugate and the dual conjugate.
|
||||
*/
|
||||
template< typename T >
|
||||
inline DualQuaternion< T > conj_dual( DualQuaternion< T > const & dq )
|
||||
{
|
||||
return DualQuaternion< T >( dq.r(), -dq.d() );
|
||||
}
|
||||
|
||||
/**
|
||||
* Quaternion conjugate. This is the conjugate that considers the dual quaternion as a quaternion with dual number elements.
|
||||
*
|
||||
* @warning There are two different types. The quaternion conjugate and the dual conjugate.
|
||||
*/
|
||||
template< typename T >
|
||||
inline DualQuaternion< T > conj_quaternion( DualQuaternion< T > const & dq )
|
||||
{
|
||||
return DualQuaternion< T >( conj( dq.r() ), conj( dq.d() ) );
|
||||
}
|
||||
|
||||
template< typename T >
|
||||
inline DualQuaternion< T > prod( DualQuaternion< T > const & q, DualQuaternion< T > const & p )
|
||||
{
|
||||
return DualQuaternion< T >( prod( q.r(), p.r() ), prod( q.r(), p.d() ) + prod( q.d(), p.r() ) );
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_MATH_DUAL_QUATERNION_H
|
||||
#endif
|
||||
@@ -0,0 +1,173 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_EIGEN_SYSTEM_DECOMPOSITION_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_EIGEN_SYSTEM_DECOMPOSITION_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <cmath> // for sqrt() and fabs
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
/**
|
||||
* Eigen System Decomposition.
|
||||
*
|
||||
* @param A Matrix to find eigenvectors and eigenvalues of.
|
||||
* @param V Upon return the columns of this matrix contains the
|
||||
* eigenvectors. IMPORTANT: V may not form a right-handed
|
||||
* coordinate system (ie. V might not be a rotation matrix). If
|
||||
* a rotation matrix is needed one should compute the determinant
|
||||
* of V and flip the sign of one of V's columns in case the
|
||||
* determinant is negative. That is:
|
||||
*
|
||||
* if(det(V)<0) { V(0,0) =- V(0,0); V(1,0) =- V(1,0); V(2,0) =- V(2,0); }
|
||||
*
|
||||
*
|
||||
* @param diag Upon return this vector contains the
|
||||
* eigenvalues, such that entry 0 correpsonds to
|
||||
* eigenvector 0 and so on.
|
||||
*/
|
||||
template<typename matrix3x3_type,typename vector3_type>
|
||||
inline void eigen(matrix3x3_type const & A,matrix3x3_type & V,vector3_type & diag)
|
||||
{
|
||||
typedef typename vector3_type::value_type real_type;
|
||||
typedef typename vector3_type::value_traits value_traits;
|
||||
|
||||
using std::sqrt;
|
||||
using std::fabs;
|
||||
|
||||
vector3_type sub_diag;
|
||||
|
||||
sub_diag.clear();
|
||||
diag.clear();
|
||||
|
||||
V = A;
|
||||
|
||||
real_type const & fM00 = V(0,0);
|
||||
real_type fM01 = V(0,1);
|
||||
real_type fM02 = V(0,2);
|
||||
real_type const & fM11 = V(1,1);
|
||||
real_type const & fM12 = V(1,2);
|
||||
real_type const & fM22 = V(2,2);
|
||||
|
||||
diag(0) = fM00;
|
||||
sub_diag(2) = value_traits::zero();
|
||||
if ( fM02 != value_traits::zero() )
|
||||
{
|
||||
real_type fLength = sqrt(fM01*fM01+fM02*fM02);
|
||||
real_type fInvLength = (value_traits::one())/fLength;
|
||||
fM01 *= fInvLength;
|
||||
fM02 *= fInvLength;
|
||||
real_type fQ = (value_traits::two())*fM01*fM12+fM02*(fM22-fM11);
|
||||
diag(1) = fM11+fM02*fQ;
|
||||
diag(2) = fM22-fM02*fQ;
|
||||
sub_diag(0) = fLength;
|
||||
sub_diag(1) = fM12-fM01*fQ;
|
||||
V(0,0) = value_traits::one();
|
||||
V(0,1) = value_traits::zero();
|
||||
V(0,2) = value_traits::zero();
|
||||
V(1,0) = value_traits::zero();
|
||||
V(1,1) = fM01;
|
||||
V(1,2) = fM02;
|
||||
V(2,0) = value_traits::zero();
|
||||
V(2,1) = fM02;
|
||||
V(2,2) = -fM01;
|
||||
}
|
||||
else
|
||||
{
|
||||
diag(1) = fM11;
|
||||
diag(2) = fM22;
|
||||
sub_diag(0) = fM01;
|
||||
sub_diag(1) = fM12;
|
||||
V(0,0) = value_traits::one();
|
||||
V(0,1) = value_traits::zero();
|
||||
V(0,2) = value_traits::zero();
|
||||
V(1,0) = value_traits::zero();
|
||||
V(1,1) = value_traits::one();
|
||||
V(1,2) = value_traits::zero();
|
||||
V(2,0) = value_traits::zero();
|
||||
V(2,1) = value_traits::zero();
|
||||
V(2,2) = value_traits::one();
|
||||
}
|
||||
|
||||
const int max_iterations = 32;
|
||||
const int dim = 3;
|
||||
for (int i0 = 0; i0 < dim; ++i0)
|
||||
{
|
||||
int i1;
|
||||
for (i1 = 0; i1 < max_iterations; ++i1)
|
||||
{
|
||||
int i2;
|
||||
for (i2 = i0; i2 <= dim-2; ++i2)
|
||||
{
|
||||
real_type fTmp = fabs(diag(i2)) + fabs(diag(i2+1));
|
||||
if ( fabs(sub_diag(i2)) + fTmp == fTmp )
|
||||
break;
|
||||
}
|
||||
if ( i2 == i0 )
|
||||
break;
|
||||
real_type fG = (diag(i0+1) - diag(i0))/(value_traits::two()* sub_diag(i0));
|
||||
real_type fR = sqrt(fG*fG+value_traits::one());
|
||||
if ( fG < value_traits::zero() )
|
||||
fG = diag(i2)-diag(i0)+sub_diag(i0)/(fG-fR);
|
||||
else
|
||||
fG = diag(i2)-diag(i0)+sub_diag(i0)/(fG+fR);
|
||||
|
||||
real_type fSin = value_traits::one();
|
||||
real_type fCos = value_traits::one();
|
||||
real_type fP = value_traits::zero();
|
||||
|
||||
for (int i3 = i2-1; i3 >= i0; --i3)
|
||||
{
|
||||
real_type fF = fSin*sub_diag(i3);
|
||||
real_type fB = fCos*sub_diag(i3);
|
||||
if ( fabs(fF) >= fabs(fG) )
|
||||
{
|
||||
fCos = fG/fF;
|
||||
fR = sqrt(fCos*fCos+value_traits::one());
|
||||
sub_diag(i3+1) = fF*fR;
|
||||
fSin = value_traits::one()/fR;
|
||||
fCos *= fSin;
|
||||
}
|
||||
else
|
||||
{
|
||||
fSin = fF/fG;
|
||||
fR = sqrt(fSin*fSin+value_traits::one());
|
||||
sub_diag(i3+1) = fG*fR;
|
||||
fCos = value_traits::one()/fR;
|
||||
fSin *= fCos;
|
||||
}
|
||||
fG = diag(i3+1)-fP;
|
||||
fR = (diag(i3)-fG)*fSin+value_traits::two()*fB*fCos;
|
||||
fP = fSin*fR;
|
||||
diag(i3+1) = fG+fP;
|
||||
fG = fCos*fR-fB;
|
||||
for (int i4 = 0; i4 < dim; ++i4)
|
||||
{
|
||||
fF = V(i4,i3+1);
|
||||
V(i4,i3+1) = fSin*V(i4,i3)+fCos*fF;
|
||||
V(i4,i3) = fCos*V(i4,i3)-fSin*fF;
|
||||
}
|
||||
}
|
||||
diag(i0) -= fP;
|
||||
sub_diag(i0) = fG;
|
||||
sub_diag(i2) = value_traits::zero();
|
||||
}
|
||||
if ( i1 == max_iterations )
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
// OPENTISSUE_CORE_MATH_MATH_EIGEN_SYSTEM_DECOMPOSITION_H
|
||||
#endif
|
||||
158
Demos3/FiniteElementMethod/OpenTissue/core/math/math_functions.h
Normal file
158
Demos3/FiniteElementMethod/OpenTissue/core/math/math_functions.h
Normal file
@@ -0,0 +1,158 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_FUNCTIONS_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_FUNCTIONS_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <OpenTissue/core/math/math_constants.h>
|
||||
|
||||
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
/**
|
||||
* The following functions are defined below:
|
||||
* - clamp (and the variations: clamp_min, clamp_max, clamp_zero_one)
|
||||
* - fac (faculty)
|
||||
* - sgn (sign function)
|
||||
* - sinc
|
||||
*/
|
||||
namespace math
|
||||
{
|
||||
|
||||
/**
|
||||
* clamp function to clamp a value to be in the interval (min_value; max_value).
|
||||
*
|
||||
* @param value The value to be clamped.
|
||||
* @param min_value The minimum allowed value.
|
||||
* @param max_value The maximum allowed value (ohhh, really?).
|
||||
* @return The clamped value. If value already is in (min_value; max_value) no clamping is performed.
|
||||
*/
|
||||
template<typename T>
|
||||
inline T clamp(T const & value, T const & min_value, T const & max_value)
|
||||
{
|
||||
assert(min_value <= max_value || !"max_value cannot be less than min_value");
|
||||
using std::min;
|
||||
using std::max;
|
||||
|
||||
return T(min(max_value, max(min_value, value)));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* clamp function to clamp a value never to be less than min_value.
|
||||
*
|
||||
* @param value The value to be clamped.
|
||||
* @param min_value The minimum allowed value.
|
||||
* @return The clamped value if value is less than min_value, otherwise the original value is returned.
|
||||
*/
|
||||
template<typename T>
|
||||
inline T clamp_min(T const & value, T const & min_value)
|
||||
{
|
||||
using std::max;
|
||||
return clamp(value, min_value, max(value, min_value));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* clamp function to clamp a value never to be greater than max_value.
|
||||
*
|
||||
* @param value The value to be clamped.
|
||||
* @param max_value The maximum allowed value.
|
||||
* @return The clamped value if value is greater than max_value, otherwise the original value is returned.
|
||||
*/
|
||||
template<typename T>
|
||||
inline T clamp_max(T const & value, T const & max_value)
|
||||
{
|
||||
using std::min;
|
||||
return clamp(value, min(value, max_value), max_value);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* clamp function to easily clamp a value between 0 and 1.
|
||||
* note: this function is mostly usable for T = some real type.
|
||||
*
|
||||
* @param value The value to be clamped.
|
||||
* @return The clamped value. If value already is in (0; 1) no clamping is performed.
|
||||
*/
|
||||
template<typename T>
|
||||
inline T clamp_zero_one(T const & value)
|
||||
{
|
||||
return clamp(value, detail::zero<T>(), detail::one<T>());
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
inline T fac(unsigned long n)
|
||||
{
|
||||
// TODO what about implicit type conversions? This could have been done more elegangtly using partial specialization
|
||||
unsigned long val = 1;
|
||||
for(; n > 0; val *= n--);
|
||||
return T(val);
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
inline T sgn(T const & val)
|
||||
{
|
||||
static T const zero = detail::zero<T>();
|
||||
static T const one = detail::one<T>();
|
||||
return val > zero ? one : val < zero ? -one : zero;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Compute Sinc Function.
|
||||
* The implementation of this method was greatly inspired by the
|
||||
* one in Open Dynamics Engine v. 0.039
|
||||
*
|
||||
* This method returns sin(x)/x. this has a singularity at 0 so special
|
||||
* handling is needed for small arguments.
|
||||
*
|
||||
* @param x
|
||||
* @return The value of sin(x)/x
|
||||
*/
|
||||
template<typename T>
|
||||
inline T sinc(T & x)
|
||||
{
|
||||
using std::fabs;
|
||||
using std::sin;
|
||||
|
||||
static T const tiny = (T)(1.0e-4);
|
||||
static T const factor = (T)(0.166666666666666666667);
|
||||
|
||||
//--- if |x| < 1e-4 then use a taylor series expansion. this two term expansion
|
||||
//--- is actually accurate to one LS bit within this range if double precision
|
||||
//--- is being used - so don't worry!
|
||||
return (fabs(x) < tiny) ? (detail::one<T>() - x*x*factor) : (sin(x)/x);
|
||||
}
|
||||
|
||||
|
||||
// use this to convert radians into degrees
|
||||
template<typename T>
|
||||
inline T to_degrees(T const & radians)
|
||||
{
|
||||
return radians*detail::radian<T>();
|
||||
}
|
||||
|
||||
|
||||
// use this to convert degrees into radians
|
||||
template<typename T>
|
||||
inline T to_radians(T const & degrees)
|
||||
{
|
||||
return degrees*detail::degree<T>();
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
// #define OPENTISSUE_CORE_MATH_MATH_FUNCTIONS_H
|
||||
#endif
|
||||
@@ -0,0 +1,65 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_INVERT4x4_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_INVERT4x4_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template<typename real_type>
|
||||
inline void invert4x4(real_type M[4][4])
|
||||
{
|
||||
real_type a00 = M[0][0]; real_type a01 = M[0][1]; real_type a02 = M[0][2]; real_type a03 = M[0][3];
|
||||
real_type a10 = M[1][0]; real_type a11 = M[1][1]; real_type a12 = M[1][2]; real_type a13 = M[1][3];
|
||||
real_type a20 = M[2][0]; real_type a21 = M[2][1]; real_type a22 = M[2][2]; real_type a23 = M[2][3];
|
||||
real_type a30 = M[3][0]; real_type a31 = M[3][1]; real_type a32 = M[3][2]; real_type a33 = M[3][3];
|
||||
|
||||
M[0][0] = a11*a22*a33 - a11*a23*a32 - a21*a12*a33 + a21*a13*a32 + a31*a12*a23 - a31*a13*a22;
|
||||
M[0][1] = - a01*a22*a33 + a01*a23*a32 + a21*a02*a33 - a21*a03*a32 - a31*a02*a23 + a31*a03*a22;
|
||||
M[0][2] = a01*a12*a33 - a01*a13*a32 - a11*a02*a33 + a11*a03*a32 + a31*a02*a13 - a31*a03*a12;
|
||||
M[0][3] = - a01*a12*a23 + a01*a13*a22 + a11*a02*a23 - a11*a03*a22 - a21*a02*a13 + a21*a03*a12;
|
||||
M[1][0] = - a10*a22*a33 + a10*a23*a32 + a20*a12*a33 - a20*a13*a32 - a30*a12*a23 + a30*a13*a22;
|
||||
M[1][1] = a00*a22*a33 - a00*a23*a32 - a20*a02*a33 + a20*a03*a32 + a30*a02*a23 - a30*a03*a22;
|
||||
M[1][2] = - a00*a12*a33 + a00*a13*a32 + a10*a02*a33 - a10*a03*a32 - a30*a02*a13 + a30*a03*a12;
|
||||
M[1][3] = a00*a12*a23 - a00*a13*a22 - a10*a02*a23 + a10*a03*a22 + a20*a02*a13 - a20*a03*a12;
|
||||
M[2][0] = a10*a21*a33 - a10*a23*a31 - a20*a11*a33 + a20*a13*a31 + a30*a11*a23 - a30*a13*a21;
|
||||
M[2][1] = - a00*a21*a33 + a00*a23*a31 + a20*a01*a33 - a20*a03*a31 - a30*a01*a23 + a30*a03*a21;
|
||||
M[2][2] = a00*a11*a33 - a00*a13*a31 - a10*a01*a33 + a10*a03*a31 + a30*a01*a13 - a30*a03*a11;
|
||||
M[2][3] = - a00*a11*a23 + a00*a13*a21 + a10*a01*a23 - a10*a03*a21 - a20*a01*a13 + a20*a03*a11;
|
||||
M[3][0] = - a10*a21*a32 + a10*a22*a31 + a20*a11*a32 - a20*a12*a31 - a30*a11*a22 + a30*a12*a21;
|
||||
M[3][1] = a00*a21*a32 - a00*a22*a31 - a20*a01*a32 + a20*a02*a31 + a30*a01*a22 - a30*a02*a21;
|
||||
M[3][2] = - a00*a11*a32 + a00*a12*a31 + a10*a01*a32 - a10*a02*a31 - a30*a01*a12 + a30*a02*a11;
|
||||
M[3][3] = a00*a11*a22 - a00*a12*a21 - a10*a01*a22 + a10*a02*a21 + a20*a01*a12 - a20*a02*a11;
|
||||
//real_type D =
|
||||
// (
|
||||
// a00*a11*a22*a33 - a00*a11*a23*a32 - a00*a21*a12*a33 + a00*a21*a13*a32 + a00*a31*a12*a23 - a00*a31*a13*a22
|
||||
// - a10*a01*a22*a33 + a10*a01*a23*a32 + a10*a21*a02*a33 - a10*a21*a03*a32 - a10*a31*a02*a23 + a10*a31*a03*a22
|
||||
// + a20*a01*a12*a33 - a20*a01*a13*a32 - a20*a11*a02*a33 + a20*a11*a03*a32 + a20*a31*a02*a13 - a20*a31*a03*a12
|
||||
// - a30*a01*a12*a23 + a30*a01*a13*a22 + a30*a11*a02*a23 - a30*a11*a03*a22 - a30*a21*a02*a13 + a30*a21*a03*a12
|
||||
// );
|
||||
real_type D = a00*M[0][0] + a10*M[0][1] + a20*M[0][2] + a30*M[0][3];
|
||||
if(D)
|
||||
{
|
||||
M[0][0] /=D; M[0][1] /=D; M[0][2] /=D; M[0][3] /=D;
|
||||
M[1][0] /=D; M[1][1] /=D; M[1][2] /=D; M[1][3] /=D;
|
||||
M[2][0] /=D; M[2][1] /=D; M[2][2] /=D; M[2][3] /=D;
|
||||
M[3][0] /=D; M[3][1] /=D; M[3][2] /=D; M[3][3] /=D;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_MATH_INVERT4x4_H
|
||||
#endif
|
||||
@@ -0,0 +1,32 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_IS_FINITE_H
|
||||
#define OPENTISSUE_CORE_MATH_IS_FINITE_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#ifdef WIN32
|
||||
#include <float.h>
|
||||
#endif
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
namespace math
|
||||
{
|
||||
|
||||
#ifdef WIN32
|
||||
#define is_finite(val) (_finite(val)!=0) ///< Is finite number test
|
||||
#else
|
||||
#define is_finite(val) (finite(val)!=0) ///< Is finite number test
|
||||
#endif
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_IS_FINITE_H
|
||||
#endif
|
||||
@@ -0,0 +1,38 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_IS_NUMBER_H
|
||||
#define OPENTISSUE_CORE_MATH_IS_NUMBER_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#ifdef WIN32
|
||||
#include <float.h>
|
||||
#else
|
||||
#include <cmath>
|
||||
#endif
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
namespace math
|
||||
{
|
||||
|
||||
#ifdef WIN32
|
||||
#define is_number(val) (_isnan(val)==0) ///< Is a number test
|
||||
#else
|
||||
#if (__APPLE__)
|
||||
#define is_number(val) (std::isnan(val)==0) ///< Is a number test
|
||||
#else
|
||||
#define is_number(val) (isnan(val)==0) ///< Is a number test
|
||||
#endif
|
||||
#endif
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_IS_NUMBER_H
|
||||
#endif
|
||||
788
Demos3/FiniteElementMethod/OpenTissue/core/math/math_matrix3x3.h
Normal file
788
Demos3/FiniteElementMethod/OpenTissue/core/math/math_matrix3x3.h
Normal file
@@ -0,0 +1,788 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_MATRIX3X3_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_MATRIX3X3_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <OpenTissue/core/math/math_vector3.h>
|
||||
#include <OpenTissue/core/math/math_value_traits.h>
|
||||
#include <OpenTissue/core/math/math_eigen_system_decomposition.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <cassert>
|
||||
#include <iosfwd>
|
||||
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template <typename > class Quaternion;
|
||||
|
||||
template<
|
||||
typename value_type_
|
||||
//, typename value_traits_ = ValueTraits<value_type_>
|
||||
>
|
||||
class Matrix3x3
|
||||
{
|
||||
protected:
|
||||
|
||||
typedef typename math::ValueTraits<value_type_> value_traits_ ; // TODO value_traits_ should be parameterized as a class template parameter.
|
||||
|
||||
public:
|
||||
|
||||
typedef value_traits_ value_traits; ///< Convience typedef to make value traits accesible for all template functions using Vector3 types.
|
||||
typedef value_type_ value_type; ///< Typedef is required for compliance with many other libraries and data containers!
|
||||
typedef Vector3<value_type> vector3_type; // TODO should Vector3 type be template parameterized?
|
||||
typedef Quaternion<value_type> quaternion_type;
|
||||
typedef size_t index_type; // TODO should the index type be template parameterized?
|
||||
|
||||
protected:
|
||||
|
||||
// TODO 2007-02-19 KE: Refactor this stuff into an array
|
||||
vector3_type m_row0; ///< The 1st row of the matrix
|
||||
vector3_type m_row1; ///< The 2nd row of the matrix
|
||||
vector3_type m_row2; ///< The 3rd row of the matrix
|
||||
|
||||
public:
|
||||
|
||||
Matrix3x3()
|
||||
: m_row0(value_traits::zero() ,value_traits::zero() ,value_traits::zero() )
|
||||
, m_row1(value_traits::zero() ,value_traits::zero() ,value_traits::zero() )
|
||||
, m_row2(value_traits::zero() ,value_traits::zero() ,value_traits::zero() )
|
||||
{}
|
||||
|
||||
~Matrix3x3(){}
|
||||
|
||||
explicit Matrix3x3(
|
||||
value_type const & m00 , value_type const & m01 , value_type const & m02
|
||||
, value_type const & m10 , value_type const & m11 , value_type const & m12
|
||||
, value_type const & m20 , value_type const & m21 , value_type const & m22
|
||||
)
|
||||
: m_row0(m00,m01,m02)
|
||||
, m_row1(m10,m11,m12)
|
||||
, m_row2(m20,m21,m22)
|
||||
{}
|
||||
|
||||
|
||||
explicit Matrix3x3(vector3_type const & row0, vector3_type const & row1, vector3_type const & row2)
|
||||
: m_row0(row0)
|
||||
, m_row1(row1)
|
||||
, m_row2(row2)
|
||||
{}
|
||||
|
||||
explicit Matrix3x3(quaternion_type const & q) { *this = q; }
|
||||
|
||||
Matrix3x3(Matrix3x3 const & M)
|
||||
: m_row0(M.m_row0)
|
||||
, m_row1(M.m_row1)
|
||||
, m_row2(M.m_row2)
|
||||
{}
|
||||
|
||||
public:
|
||||
|
||||
value_type & operator()(index_type i, index_type j)
|
||||
{
|
||||
assert( ( i>=0 && i<3 ) || !"Matrix3x3::(i,j) i must be in range [0..2]");
|
||||
assert( ( j>=0 && j<3 ) || !"Matrix3x3::(i,j) j must be in range [0..2]");
|
||||
return (*(&m_row0+i))(j);
|
||||
}
|
||||
|
||||
value_type const & operator()(index_type i, index_type j) const
|
||||
{
|
||||
assert( ( i>=0 && i<3 ) || !"Matrix3x3::(i,j) i must be in range [0..2]");
|
||||
assert( ( j>=0 && j<3 ) || !"Matrix3x3::(i,j) j must be in range [0..2]");
|
||||
return (*(&m_row0+i))(j);
|
||||
}
|
||||
|
||||
vector3_type & operator[](index_type i)
|
||||
{
|
||||
assert( ( i>=0 && i<3 ) || !"Matrix3x3::(i,j) i must be in range [0..2]");
|
||||
return *(&m_row0+i);
|
||||
}
|
||||
|
||||
vector3_type const & operator[](index_type i) const
|
||||
{
|
||||
assert( ( i>=0 && i<3 ) || !"Matrix3x3::(i,j) i must be in range [0..2]");
|
||||
return *(&m_row0+i);
|
||||
}
|
||||
|
||||
Matrix3x3 & operator=( Matrix3x3 const & cpy )
|
||||
{
|
||||
m_row0=cpy.m_row0;
|
||||
m_row1=cpy.m_row1;
|
||||
m_row2=cpy.m_row2;
|
||||
return *this;
|
||||
}
|
||||
|
||||
// TODO: Comparing floats with == or != is not safe NOTE T might not be a float type it could be anything? This suggest that we need some kinf of metaprogramming technique to deal with ths problem?
|
||||
bool operator==(Matrix3x3 const & cmp ) const
|
||||
{
|
||||
return (m_row0==cmp.m_row0) && (m_row1==cmp.m_row1) && (m_row2==cmp.m_row2);
|
||||
}
|
||||
|
||||
bool operator!=( Matrix3x3 const & cmp ) const
|
||||
{
|
||||
return !(*this==cmp);
|
||||
}
|
||||
|
||||
Matrix3x3 operator+ ( Matrix3x3 const & m ) const { return Matrix3x3( m_row0+m.m_row0, m_row1+m.m_row1, m_row2+m.m_row2); }
|
||||
Matrix3x3 operator- ( Matrix3x3 const & m ) const { return Matrix3x3( m_row0-m.m_row0, m_row1-m.m_row1, m_row2-m.m_row2); }
|
||||
Matrix3x3 & operator+= ( Matrix3x3 const & m ) { m_row0+=m.m_row0; m_row1+=m.m_row1; m_row2+=m.m_row2; return *this; }
|
||||
Matrix3x3 & operator-= ( Matrix3x3 const & m ) { m_row0-=m.m_row0; m_row1-=m.m_row1; m_row2-=m.m_row2; return *this; }
|
||||
Matrix3x3 & operator*= ( value_type const & s ) {m_row0*=s;m_row1*=s;m_row2*=s;return *this;}
|
||||
|
||||
Matrix3x3 & operator/= ( value_type const & s )
|
||||
{
|
||||
assert(s || !"Matrix3x3/=(): division by zero");
|
||||
m_row0/=s;
|
||||
m_row1/=s;
|
||||
m_row2/=s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
vector3_type operator*(vector3_type const &v) const { return vector3_type(m_row0*v, m_row1*v, m_row2*v); }
|
||||
Matrix3x3 operator-() const { return Matrix3x3(-m_row0,-m_row1,-m_row2); }
|
||||
|
||||
size_t size1() const { return 3u; }
|
||||
size_t size2() const { return 3u; }
|
||||
|
||||
/**
|
||||
* Assigns this quaternion to a matrix.
|
||||
* This method performs a conversion of a quaternion that represents a rotation into the correponding rotationmatrix.
|
||||
*
|
||||
* @param q A reference to a quaternion. This is the quaternion that represents a rotation.
|
||||
*/
|
||||
Matrix3x3 & operator=(quaternion_type const & q)
|
||||
{
|
||||
m_row0(0) = value_traits::one() - value_traits::two() * ( (q.v()(1)*q.v()(1)) + (q.v()(2)*q.v()(2)));
|
||||
m_row1(1) = value_traits::one() - value_traits::two() * ( (q.v()(0)*q.v()(0)) + (q.v()(2)*q.v()(2)));
|
||||
m_row2(2) = value_traits::one() - value_traits::two() * ( (q.v()(1)*q.v()(1)) + (q.v()(0)*q.v()(0)));
|
||||
m_row1(0) = value_traits::two() * ( (q.v()(0)*q.v()(1)) + (q.s()*q.v()(2)));
|
||||
m_row0(1) = value_traits::two() * ( (q.v()(0)*q.v()(1)) - (q.s()*q.v()(2)));
|
||||
m_row2(0) = value_traits::two() * (-(q.s()*q.v()(1)) + (q.v()(0)*q.v()(2)));
|
||||
m_row0(2) = value_traits::two() * ( (q.s()*q.v()(1)) + (q.v()(0)*q.v()(2)));
|
||||
m_row2(1) = value_traits::two() * ( (q.v()(2)*q.v()(1)) + (q.s()*q.v()(0)));
|
||||
m_row1(2) = value_traits::two() * ( (q.v()(2)*q.v()(1)) - (q.s()*q.v()(0)));
|
||||
return *this;
|
||||
}
|
||||
|
||||
void clear()
|
||||
{
|
||||
m_row0.clear();
|
||||
m_row1.clear();
|
||||
m_row2.clear();
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
friend Matrix3x3 fabs(Matrix3x3 const & A)
|
||||
{
|
||||
using std::fabs;
|
||||
|
||||
return Matrix3x3(
|
||||
fabs(A(0,0)), fabs(A(0,1)), fabs(A(0,2)),
|
||||
fabs(A(1,0)), fabs(A(1,1)), fabs(A(1,2)),
|
||||
fabs(A(2,0)), fabs(A(2,1)), fabs(A(2,2))
|
||||
);
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
vector3_type column(index_type i) const
|
||||
{
|
||||
assert((i>=0 && i<3) || !"Matrix3x3::column(): index must be in range [0..2]");
|
||||
|
||||
return vector3_type(m_row0(i), m_row1(i), m_row2(i));
|
||||
}
|
||||
|
||||
void set_column(index_type i, vector3_type const & column)
|
||||
{
|
||||
assert((i>=0 && i<3) || !"Matrix3x3::set_column(): index must be in range [0..2]");
|
||||
|
||||
m_row0(i) = column(0);
|
||||
m_row1(i) = column(1);
|
||||
m_row2(i) = column(2);
|
||||
}
|
||||
|
||||
vector3_type & row(index_type i) { return (*this)[i]; }
|
||||
vector3_type const & row(index_type i) const { return (*this)[i]; }
|
||||
|
||||
}; // class Matrix3x3
|
||||
|
||||
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> operator*( Matrix3x3<T> const & m, T const &s )
|
||||
{
|
||||
return Matrix3x3<T>(m.row(0)*s, m.row(1)*s, m.row(2)*s);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> operator*( T const & s, Matrix3x3<T> const & m )
|
||||
{
|
||||
return Matrix3x3<T>(m.row(0)*s, m.row(1)*s, m.row(2)*s);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> operator/( Matrix3x3<T> const & m, T const & s )
|
||||
{
|
||||
return Matrix3x3<T>(m.row(0)/s, m.row(1)/s, m.row(2)/s);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> operator/( T const & s, Matrix3x3<T> const & m )
|
||||
{
|
||||
return Matrix3x3<T>(m.row(0)/s, m.row(1)/s, m.row(2)/s);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> operator*(Matrix3x3<T> const & A,Matrix3x3<T> const & B)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
Matrix3x3<T> C;
|
||||
|
||||
for (int c=0; c<3; ++c)
|
||||
for (int r=0; r<3; ++r){
|
||||
C(r,c) = value_traits::zero();
|
||||
for(int i=0; i<3; ++i){
|
||||
C(r,c) += A(r,i) * B(i,c);
|
||||
}
|
||||
}
|
||||
return C;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a rotation matrix.
|
||||
* This method returns a rotation matrix around the x-axe. It
|
||||
* assumes that post-multiplication by colum vectors is used.
|
||||
*
|
||||
* @param radians The rotation angle in radians.
|
||||
*/
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> Rx(T const & radians)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
T cosinus = (T)( cos(radians) );
|
||||
T sinus = (T)( sin(radians) );
|
||||
|
||||
return Matrix3x3<T>(
|
||||
value_traits::one(), value_traits::zero(), value_traits::zero(),
|
||||
value_traits::zero(), cosinus, -sinus,
|
||||
value_traits::zero(), sinus, cosinus
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a rotation matrix.
|
||||
* This method returns a rotation matrix around the y-axe. It
|
||||
* assumes that post-multiplication by colum vectors is used.
|
||||
*
|
||||
* @param radians The rotation angle in radians.
|
||||
*/
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> Ry(T const & radians)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
T cosinus = (T)( cos(radians) );
|
||||
T sinus = (T)( sin(radians) );
|
||||
return Matrix3x3<T>(
|
||||
cosinus, value_traits::zero(), sinus,
|
||||
value_traits::zero(), value_traits::one(), value_traits::zero(),
|
||||
-sinus, value_traits::zero(), cosinus
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a rotation matrix.
|
||||
* This method returns a rotation matrix around the z-axe. It assumes that post-multiplication by colum vectors is used.
|
||||
*
|
||||
* @param radians The rotation angle in radians.
|
||||
*/
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> Rz(T const & radians)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
T cosinus = (T)( cos(radians) );
|
||||
T sinus = (T)( sin(radians) );
|
||||
|
||||
return Matrix3x3<T>(
|
||||
cosinus, -sinus, value_traits::zero(),
|
||||
sinus, cosinus, value_traits::zero(),
|
||||
value_traits::zero(), value_traits::zero(), value_traits::one()
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a rotation matrix.
|
||||
* This method returns a general rotation matrix around a specified axe. It assumes that post-multiplication by colum vectors is used.
|
||||
*
|
||||
* @param radians The rotation angle in radians.
|
||||
* @param axe A vector. This is the rotation axe.
|
||||
*/
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> Ru(T const & radians, Vector3<T> const & axis)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
T cosinus = (T)( cos(radians) );
|
||||
T sinus = (T)( sin(radians) );
|
||||
Vector3<T> u = unit(axis);
|
||||
|
||||
//Foley p.227 (5.76)
|
||||
return Matrix3x3<T>(
|
||||
u(0)*u(0) + cosinus*(value_traits::one() - u(0)*u(0)), u(0)*u(1)*(value_traits::one()-cosinus) - sinus*u(2), u(0)*u(2)*(value_traits::one()-cosinus) + sinus*u(1),
|
||||
u(0)*u(1)*(value_traits::one()-cosinus) + sinus*u(2), u(1)*u(1) + cosinus*(value_traits::one() - u(1)*u(1)), u(1)*u(2)*(value_traits::one()-cosinus) - sinus*u(0),
|
||||
u(0)*u(2)*(value_traits::one()-cosinus) - sinus*u(1), u(1)*u(2)*(value_traits::one()-cosinus) + sinus*u(0), u(2)*u(2) + cosinus*(value_traits::one() - u(2)*u(2))
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Direction of Flight (DoF)
|
||||
*
|
||||
* @param k The desired direction of flight.
|
||||
*/
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> z_dof(Vector3<T> const & k)
|
||||
{
|
||||
Vector3<T> i,j;
|
||||
orthonormal_vectors(i,j,unit(k));
|
||||
return Matrix3x3<T>(
|
||||
i(0) , j(0), k(0)
|
||||
, i(1) , j(1), k(1)
|
||||
, i(2) , j(2), k(2)
|
||||
);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline bool is_orthonormal(Matrix3x3<T> const & M,T const & threshold)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
using std::fabs;
|
||||
|
||||
assert(threshold>=value_traits::zero() || !"is_orthonormal(): threshold must be non-negative");
|
||||
|
||||
T dot01 = M.m_row0*M.m_row1;
|
||||
T dot02 = M.m_row0*M.m_row2;
|
||||
T dot12 = M.m_row1*M.m_row2;
|
||||
if(fabs(dot01)>threshold) return false;
|
||||
if(fabs(dot02)>threshold) return false;
|
||||
if(fabs(dot12)>threshold) return false;
|
||||
T dot00 = M.m_row0*M.m_row0;
|
||||
T dot11 = M.m_row1*M.m_row1;
|
||||
T dot22 = M.m_row2*M.m_row2;
|
||||
if((dot00-value_traits::one())>threshold) return false;
|
||||
if((dot11-value_traits::one())>threshold) return false;
|
||||
if((dot22-value_traits::one())>threshold) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline bool is_orthonormal(Matrix3x3<T> const & M)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
return is_orthonormal(M, value_traits::zero());
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline bool is_zero(Matrix3x3<T> M, T const & threshold)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
using std::fabs;
|
||||
|
||||
assert(threshold>=value_traits::zero() || !"is_zero(): threshold must be non-negative");
|
||||
|
||||
if(fabs(M(0,0))>threshold) return false;
|
||||
if(fabs(M(0,1))>threshold) return false;
|
||||
if(fabs(M(0,2))>threshold) return false;
|
||||
if(fabs(M(1,0))>threshold) return false;
|
||||
if(fabs(M(1,1))>threshold) return false;
|
||||
if(fabs(M(1,2))>threshold) return false;
|
||||
if(fabs(M(2,0))>threshold) return false;
|
||||
if(fabs(M(2,1))>threshold) return false;
|
||||
if(fabs(M(2,2))>threshold) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline bool is_zero(Matrix3x3<T> const & M)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
return is_zero(M, value_traits::zero());
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline bool is_symmetric(Matrix3x3<T> M, T const & threshold)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
using std::fabs;
|
||||
|
||||
assert(threshold>=value_traits::zero() || !"is_symmetric(): threshold must be non-negative");
|
||||
|
||||
if(fabs(M(0,1)-M(1,0))>threshold) return false;
|
||||
if(fabs(M(0,2)-M(2,0))>threshold) return false;
|
||||
if(fabs(M(1,2)-M(2,1))>threshold) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline bool is_symmetric(Matrix3x3<T> const & M)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
return is_symmetric(M, value_traits::zero());
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline bool is_diagonal(Matrix3x3<T> M, T const & threshold)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
using std::fabs;
|
||||
|
||||
assert(threshold>=value_traits::zero() || !"is_diagonal(): threshold must be non-negative");
|
||||
|
||||
if(fabs(M(0,1))>threshold) return false;
|
||||
if(fabs(M(0,2))>threshold) return false;
|
||||
if(fabs(M(1,0))>threshold) return false;
|
||||
if(fabs(M(1,2))>threshold) return false;
|
||||
if(fabs(M(2,0))>threshold) return false;
|
||||
if(fabs(M(2,1))>threshold) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline bool is_diagonal(Matrix3x3<T> const & M)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
return is_diagonal(M, value_traits::zero());
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline bool is_identity(Matrix3x3<T> M, T const & threshold)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
using std::fabs;
|
||||
|
||||
assert(threshold>=value_traits::zero() || !"is_identity(): threshold must be non-negative");
|
||||
|
||||
if(fabs(M(0,0)-value_traits::one())>threshold) return false;
|
||||
if(fabs(M(0,1))>threshold) return false;
|
||||
if(fabs(M(0,2))>threshold) return false;
|
||||
if(fabs(M(1,0))>threshold) return false;
|
||||
if(fabs(M(1,1)-value_traits::one())>threshold) return false;
|
||||
if(fabs(M(1,2))>threshold) return false;
|
||||
if(fabs(M(2,0))>threshold) return false;
|
||||
if(fabs(M(2,1))>threshold) return false;
|
||||
if(fabs(M(2,2)-value_traits::one())>threshold) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline bool is_identity(Matrix3x3<T> const & M)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
return is_identity(M, value_traits::zero());
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline Matrix3x3<T> outer_prod( Vector3<T> const & v1, Vector3<T> const & v2 )
|
||||
{
|
||||
return Matrix3x3<T>(
|
||||
( v1( 0 ) * v2( 0 ) ), ( v1( 0 ) * v2( 1 ) ), ( v1( 0 ) * v2( 2 ) ),
|
||||
( v1( 1 ) * v2( 0 ) ), ( v1( 1 ) * v2( 1 ) ), ( v1( 1 ) * v2( 2 ) ),
|
||||
( v1( 2 ) * v2( 0 ) ), ( v1( 2 ) * v2( 1 ) ), ( v1( 2 ) * v2( 2 ) )
|
||||
);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> ortonormalize(Matrix3x3<T> const & A)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::vector3_type vector3_type;
|
||||
|
||||
vector3_type row0(A(0,0),A(0,1),A(0,2));
|
||||
vector3_type row1(A(1,0),A(1,1),A(1,2));
|
||||
vector3_type row2(A(2,0),A(2,1),A(2,2));
|
||||
|
||||
T const l0 = length(row0);
|
||||
if(l0) row0 /= l0;
|
||||
|
||||
row1 -= row0 * dot(row0 , row1);
|
||||
T const l1 = length(row1);
|
||||
if(l1) row1 /= l1;
|
||||
|
||||
row2 = cross( row0 , row1);
|
||||
|
||||
return Matrix3x3<T>(
|
||||
row0(0), row0(1), row0(2),
|
||||
row1(0), row1(1), row1(2),
|
||||
row2(0), row2(1), row2(2)
|
||||
);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> trans(Matrix3x3<T> const & M)
|
||||
{
|
||||
return Matrix3x3<T>(
|
||||
M(0,0), M(1,0), M(2,0),
|
||||
M(0,1), M(1,1), M(2,1),
|
||||
M(0,2), M(1,2), M(2,2)
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> diag(T const & d0,T const & d1,T const & d2 )
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
return Matrix3x3<T>(
|
||||
d0, value_traits::zero(), value_traits::zero(),
|
||||
value_traits::zero(), d1, value_traits::zero(),
|
||||
value_traits::zero(), value_traits::zero(), d2
|
||||
);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> diag(Vector3<T> const & d) { return diag( d(0), d(1), d(2)); }
|
||||
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> diag(T const & d ) { return diag(d,d,d); }
|
||||
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> inverse(Matrix3x3<T> const & A)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
//From messer p.283 we know
|
||||
//
|
||||
// -1 1
|
||||
// A = ----- adj A
|
||||
// det A
|
||||
//
|
||||
// i+j
|
||||
// ij-cofactor of A = -1 det A
|
||||
// ij
|
||||
//
|
||||
// i,j entry of the adjoint.
|
||||
// i+j
|
||||
// adjoint A = ji-cofactor = A = -1 det A
|
||||
// ij ji
|
||||
//
|
||||
// As it can be seen the only numerical error
|
||||
// in these calculations is from the resolution
|
||||
// of the scalars. So it is a very accurate method.
|
||||
//
|
||||
Matrix3x3<T> adj;
|
||||
adj(0,0) = A(1,1)*A(2,2) - A(2,1)*A(1,2);
|
||||
adj(1,1) = A(0,0)*A(2,2) - A(2,0)*A(0,2);
|
||||
adj(2,2) = A(0,0)*A(1,1) - A(1,0)*A(0,1);
|
||||
adj(0,1) = A(1,0)*A(2,2) - A(2,0)*A(1,2);
|
||||
adj(0,2) = A(1,0)*A(2,1) - A(2,0)*A(1,1);
|
||||
adj(1,0) = A(0,1)*A(2,2) - A(2,1)*A(0,2);
|
||||
adj(1,2) = A(0,0)*A(2,1) - A(2,0)*A(0,1);
|
||||
adj(2,0) = A(0,1)*A(1,2) - A(1,1)*A(0,2);
|
||||
adj(2,1) = A(0,0)*A(1,2) - A(1,0)*A(0,2);
|
||||
T det = A(0,0)*adj(0,0) - A(0,1)*adj(0,1) + A(0,2)*adj(0,2);
|
||||
if(det)
|
||||
{
|
||||
adj(0,1) = -adj(0,1);
|
||||
adj(1,0) = -adj(1,0);
|
||||
adj(1,2) = -adj(1,2);
|
||||
adj(2,1) = -adj(2,1);
|
||||
return trans(adj)/det;
|
||||
}
|
||||
|
||||
return diag(value_traits::one());
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline T max_value(Matrix3x3<T> const & A)
|
||||
{
|
||||
using std::max;
|
||||
|
||||
return max(A(0,0), max( A(0,1), max( A(0,2), max ( A(1,0), max ( A(1,1), max ( A(1,2), max (A(2,0), max ( A(2,1) , A(2,2) ) ) ) ) ) ) ) );
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline T min_value(Matrix3x3<T> const & A)
|
||||
{
|
||||
using std::min;
|
||||
|
||||
return min(A(0,0), min( A(0,1), min( A(0,2), min ( A(1,0), min ( A(1,1), min ( A(1,2), min (A(2,0), min ( A(2,1) , A(2,2) ) ) ) ) ) ) ) );
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline T det(Matrix3x3<T> const & A)
|
||||
{
|
||||
return A(0,0)*(A(1,1)*A(2,2) - A(2,1)*A(1,2)) - A(0,1)*(A(1,0)*A(2,2) - A(2,0)*A(1,2)) + A(0,2)*(A(1,0)*A(2,1) - A(2,0)*A(1,1));
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline T trace(Matrix3x3<T> const & A)
|
||||
{
|
||||
return (A(0,0) + A(1,1) + A(2,2));
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline T norm_1(Matrix3x3<T> const & A)
|
||||
{
|
||||
using std::fabs;
|
||||
using std::max;
|
||||
|
||||
T r0 = fabs(A(0,0)) + fabs(A(0,1)) + fabs(A(0,2));
|
||||
T r1 = fabs(A(1,0)) + fabs(A(1,1)) + fabs(A(1,2));
|
||||
T r2 = fabs(A(2,0)) + fabs(A(2,1)) + fabs(A(2,2));
|
||||
|
||||
return max ( r0, max( r1, r2 ) );
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline T norm_2(Matrix3x3<T> const & A)
|
||||
{
|
||||
using std::fabs;
|
||||
using std::max;
|
||||
using std::sqrt;
|
||||
|
||||
Matrix3x3<T> V;
|
||||
typename Matrix3x3<T>::vector3_type d;
|
||||
math::eigen(A,V,d);
|
||||
|
||||
T lambda = max( fabs(d(0)), max( fabs(d(1)) , fabs(d(2)) ));
|
||||
return sqrt( lambda );
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline T norm_inf(Matrix3x3<T> const & A)
|
||||
{
|
||||
using std::fabs;
|
||||
using std::max;
|
||||
|
||||
T c0 = fabs(A(0,0)) + fabs(A(1,0)) + fabs(A(2,0));
|
||||
T c1 = fabs(A(0,1)) + fabs(A(1,1)) + fabs(A(2,1));
|
||||
T c2 = fabs(A(0,2)) + fabs(A(1,2)) + fabs(A(2,2));
|
||||
return max ( c0, max( c1, c2 ) );
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> truncate(Matrix3x3<T> const & A,T const & epsilon)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
using std::fabs;
|
||||
|
||||
assert(epsilon>value_traits::zero() || !"truncate(Matrix3x3,epsilon): epsilon must be positive");
|
||||
|
||||
return Matrix3x3<T>(
|
||||
fabs(A(0,0))<epsilon?value_traits::zero():A(0,0),
|
||||
fabs(A(0,1))<epsilon?value_traits::zero():A(0,1),
|
||||
fabs(A(0,2))<epsilon?value_traits::zero():A(0,2),
|
||||
|
||||
fabs(A(1,0))<epsilon?value_traits::zero():A(1,0),
|
||||
fabs(A(1,1))<epsilon?value_traits::zero():A(1,1),
|
||||
fabs(A(1,2))<epsilon?value_traits::zero():A(1,2),
|
||||
|
||||
fabs(A(2,0))<epsilon?value_traits::zero():A(2,0),
|
||||
fabs(A(2,1))<epsilon?value_traits::zero():A(2,1),
|
||||
fabs(A(2,2))<epsilon?value_traits::zero():A(2,2)
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets up the cross product matrix.
|
||||
* This method is usefull for expression the cross product as a matrix multiplication.
|
||||
*
|
||||
* @param v A reference to a vector. This is first argument of the cross product.
|
||||
*/
|
||||
template<typename T>
|
||||
inline Matrix3x3<T> star(Vector3<T> const & v)
|
||||
{
|
||||
typedef typename Matrix3x3<T>::value_traits value_traits;
|
||||
|
||||
//--- Changes a cross-product into a matrix multiplication.
|
||||
//--- Rewrites the component of a vector3_type cross-product as a matrix.
|
||||
//--- a x b = a*b = ba*
|
||||
return Matrix3x3<T>(
|
||||
value_traits::zero(), -v(2), v(1),
|
||||
v(2), value_traits::zero(), -v(0),
|
||||
-v(1), v(0), value_traits::zero()
|
||||
);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline std::ostream & operator<< (std::ostream & o,Matrix3x3<T> const & A)
|
||||
{
|
||||
o << "[" << A(0,0)
|
||||
<< "," << A(0,1)
|
||||
<< "," << A(0,2)
|
||||
<< ";" << A(1,0)
|
||||
<< "," << A(1,1)
|
||||
<< "," << A(1,2)
|
||||
<< ";" << A(2,0)
|
||||
<< "," << A(2,1)
|
||||
<< "," << A(2,2)
|
||||
<< "]";
|
||||
return o;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline std::istream & operator>>(std::istream & i,Matrix3x3<T> & A)
|
||||
{
|
||||
char dummy;
|
||||
i >> dummy;
|
||||
i >> A(0,0);
|
||||
i >> dummy;
|
||||
i >> A(0,1);
|
||||
i >> dummy;
|
||||
i >> A(0,2);
|
||||
i >> dummy;
|
||||
i >> A(1,0);
|
||||
i >> dummy;
|
||||
i >> A(1,1);
|
||||
i >> dummy;
|
||||
i >> A(1,2);
|
||||
i >> dummy;
|
||||
i >> A(2,0);
|
||||
i >> dummy;
|
||||
i >> A(2,1);
|
||||
i >> dummy;
|
||||
i >> A(2,2);
|
||||
i >> dummy;
|
||||
return i;
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_MATH_MATRIX3X3_H
|
||||
#endif
|
||||
@@ -0,0 +1,149 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_POLAR_DECOMPOSITION_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_POLAR_DECOMPOSITION_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <OpenTissue/core/math/math_vector3.h>
|
||||
#include <OpenTissue/core/math/math_matrix3x3.h>
|
||||
#include <OpenTissue/core/math/math_eigen_system_decomposition.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
// TODO: [micky] don't introduce yet another sub namespace to avoid name clashing.
|
||||
// Rename the function 'eigen' instead!!
|
||||
namespace polar_decomposition
|
||||
{
|
||||
/*
|
||||
* Polar Decomposition of matrix A (as described by Etzmuss et. al in ``A Fast Finite Solution for Cloth Modelling'')
|
||||
*
|
||||
* A = R S
|
||||
*
|
||||
* Where R is a special orthogonal matrix (R*R^T = I and det(R)=1)
|
||||
*
|
||||
* S^2 = A^T A
|
||||
*
|
||||
* let d be vector of eigenvalues and let v_0,v_1, and v_2 be corresponding eigenvectors of (A^T A), then
|
||||
*
|
||||
* S = sqrt(d_0) v_0 * v_0^T + ... + sqrt(d_2) v_2 * v_2^T
|
||||
*
|
||||
* Now compute
|
||||
*
|
||||
* R = A * S^-1
|
||||
*
|
||||
*
|
||||
* @return If the decompostion is succesfull then the return value is true otherwise it is false.
|
||||
*/
|
||||
template<typename matrix3x3_type>
|
||||
inline bool eigen(matrix3x3_type const & A,matrix3x3_type & R,matrix3x3_type & S)
|
||||
{
|
||||
using std::sqrt;
|
||||
|
||||
// A = U D V^T // A is square so: thin SVD = SVD
|
||||
// A = (U V^T) (V D V^T)
|
||||
// = R S
|
||||
//
|
||||
//Notice S is symmetric R should be orthonormal?
|
||||
//
|
||||
// proof of
|
||||
// S = sqrt( A^T A )
|
||||
//
|
||||
// start by
|
||||
//
|
||||
// S * S = A^T A
|
||||
// V D V^T V D V^T = V D U^T U D V^T
|
||||
// V D D V^T = V D D V^T
|
||||
//Assume
|
||||
//A = R S
|
||||
//pre-multiply and use assumption then
|
||||
// A^T A = A^T R S
|
||||
// A^T A = S^T S = S S
|
||||
// last step used S is symmetric
|
||||
typedef typename matrix3x3_type::vector3_type vector3_type;
|
||||
typedef typename matrix3x3_type::value_traits value_traits;
|
||||
matrix3x3_type V;
|
||||
vector3_type d;
|
||||
matrix3x3_type S2 = trans(A)*A;
|
||||
eigen(S2,V,d);
|
||||
|
||||
//--- Test if all eigenvalues are positive
|
||||
if( d(0) <= value_traits::zero() || d(1) <= value_traits::zero() || d(2) <= value_traits::zero() )
|
||||
return false;
|
||||
|
||||
vector3_type v0 = vector3_type( V(0,0), V(1,0), V(2,0) );
|
||||
vector3_type v1 = vector3_type( V(0,1), V(1,1), V(2,1) );
|
||||
vector3_type v2 = vector3_type( V(0,2), V(1,2), V(2,2) );
|
||||
S = outer_prod(v0,v0)* sqrt(d(0)) + outer_prod(v1,v1)* sqrt(d(1)) + outer_prod(v2,v2)* sqrt(d(2));
|
||||
R = A * inverse(S);
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* This method is rather bad, it is iterative and there is no way of telling how good the solution is. thus we recommend the eigen method for polar decomposition.
|
||||
*
|
||||
* Polar Decomposition as described by Shoemake and Duff in ``Matrix Animation and Polar Decomposition''
|
||||
*
|
||||
* A = R S
|
||||
*
|
||||
* Where R is a special orthogonal matrix (R*R^T = I and det(R)=1)
|
||||
*
|
||||
* Set R_0 = A
|
||||
* Do
|
||||
* R_{i+1} = 1/2( R_i + R^{-T}_i )
|
||||
* Until R_{i+1}-R_i = 0
|
||||
*
|
||||
*
|
||||
* @return If a solution was found within the specified threshold value then the return value is true otherwise it is false.
|
||||
*/
|
||||
template<typename matrix3x3_type>
|
||||
inline bool newton(matrix3x3_type const & A, matrix3x3_type & R,unsigned int max_iterations, typename matrix3x3_type::value_type const & threshold)
|
||||
{
|
||||
typedef typename matrix3x3_type::value_traits value_traits;
|
||||
typedef typename matrix3x3_type::value_type real_type;
|
||||
|
||||
assert(max_iterations>0 || !"polar_decompostion::newton() max_iterations must be positive");
|
||||
assert(threshold>value_traits::zero() || !"polar_decomposition::newton(): theshold must be positive");
|
||||
|
||||
matrix3x3_type Q[2];
|
||||
int cur = 0, next = 1;
|
||||
Q[cur] = A;
|
||||
|
||||
bool within_threshold = false;
|
||||
|
||||
for(unsigned int iteration=0;iteration< max_iterations;++iteration)
|
||||
{
|
||||
Q[next] = ( Q[cur] + trans(inverse(Q[cur])) )*.5;
|
||||
real_type test = max_value ( Q[next] - Q[cur] );
|
||||
if( test < threshold )
|
||||
{
|
||||
within_threshold = true;
|
||||
break;
|
||||
}
|
||||
cur = (cur + 1)%2;
|
||||
next = (next + 1)%2;
|
||||
}
|
||||
R = Q[next];
|
||||
return within_threshold;
|
||||
}
|
||||
|
||||
}// namespace polar_decomposition
|
||||
|
||||
}// namespace math
|
||||
|
||||
}// namespace OpenTissue
|
||||
|
||||
// OPENTISSUE_CORE_MATH_MATH_POLAR_DECOMPOSITION_H
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,83 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_POWER2_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_POWER2_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
/**
|
||||
* This method test whether there exist some positive integer, n, such that
|
||||
*
|
||||
* 2^n == val
|
||||
*
|
||||
* In which case it returns true otherwise it returns false.
|
||||
*/
|
||||
template< class T>
|
||||
inline bool is_power2( T val )
|
||||
{
|
||||
T next = 1u;
|
||||
for ( unsigned int i = 0u; i < 32u; ++i )
|
||||
{
|
||||
if ( val == next )
|
||||
return true;
|
||||
next = next << 1u;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* This function finds the smallest positive integer, n, such that
|
||||
*
|
||||
* val <= 2^n
|
||||
*
|
||||
* and returns the value 2^n.
|
||||
*/
|
||||
template<class T>
|
||||
inline T upper_power2( T val )
|
||||
{
|
||||
T next = 1u;
|
||||
for ( unsigned int i = 0u; i < 32u; ++i )
|
||||
{
|
||||
if ( next >= val )
|
||||
return next;
|
||||
next = next << 1u;
|
||||
}
|
||||
return 0u;
|
||||
}
|
||||
|
||||
/**
|
||||
* This function finds the largest positive integer, n, such that
|
||||
*
|
||||
* 2^n <= val
|
||||
*
|
||||
* and returns the value 2^n.
|
||||
*/
|
||||
template<class T>
|
||||
inline T lower_power2( T val )
|
||||
{
|
||||
T next = 1u << 31u;
|
||||
for ( unsigned int i = 0u; i < 32u; ++i )
|
||||
{
|
||||
if ( next <= val )
|
||||
return next;
|
||||
next = next >> 1u;
|
||||
}
|
||||
return 0u;
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_MATH_POWER2_H
|
||||
#endif
|
||||
@@ -0,0 +1,42 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_PRECISION_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_PRECISION_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <limits> // for std::numeric_limits<T>::epsilon()
|
||||
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
template <typename T>
|
||||
inline T machine_precision()
|
||||
{
|
||||
return std::numeric_limits<T>::epsilon();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline T working_precision()
|
||||
{
|
||||
return std::numeric_limits<T>::epsilon()*10;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline T working_precision(unsigned int scale_factor)
|
||||
{
|
||||
return std::numeric_limits<T>::epsilon()*scale_factor;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//OPENTISSUE_CORE_MATH_MATH_PRECISION_H
|
||||
#endif
|
||||
@@ -0,0 +1,209 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_PRIME_NUMBERS_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_PRIME_NUMBERS_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <OpenTissue/core/math/math_constants.h>
|
||||
|
||||
#include <cmath> //--- for floor and sqrt
|
||||
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
|
||||
namespace detail
|
||||
{
|
||||
|
||||
/**
|
||||
* Used exclusively by the Miller-Rabin algorithm. It
|
||||
* is essential the same as modular exponentiation a
|
||||
* raised to n-1 modulo n.
|
||||
*
|
||||
* This "version" is specialized to test for notrivial
|
||||
* square roots of 1.
|
||||
*/
|
||||
inline bool withness(int a,int n)
|
||||
{
|
||||
int ndec = n-1;int d = 1; int k = ndec;
|
||||
if(k!=0)
|
||||
{
|
||||
int c=0;
|
||||
while((k&0x80000000)==0)
|
||||
k<<=1;c++;
|
||||
while(c<32)
|
||||
{
|
||||
int x =d;
|
||||
d=(d*d)%n;
|
||||
if((d==1)&&(x!=1)&&(x!=ndec))
|
||||
return true;//--- Notrival square root of 1 was discovered.
|
||||
if((k&0x80000000)!=0)
|
||||
d=(d*a)%n;
|
||||
k<<=1;c++;
|
||||
}
|
||||
}
|
||||
if(d!=1)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
}//namespace detail
|
||||
|
||||
|
||||
/**
|
||||
* Prime Test.
|
||||
* This method tests if the specified integer is a prime.
|
||||
*
|
||||
* Do Always find a correct solution, but is
|
||||
* very slow for large numbers.
|
||||
*
|
||||
* Runs in time 2^(beta/2) where beta is the
|
||||
* number of bits of n.
|
||||
*
|
||||
* @param n The number to test.
|
||||
* @return If n was prime then the return value is
|
||||
* true otherwise it is false.
|
||||
*/
|
||||
inline bool trial_division(int n)
|
||||
{
|
||||
using std::sqrt;
|
||||
using std::floor;
|
||||
|
||||
double sqrN = sqrt(static_cast<double>(n));
|
||||
int j = static_cast<int>( floor(sqrN) );
|
||||
for(int i=2;i<=j;++i)
|
||||
{
|
||||
if((n%i)==0)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Modular Exponentiation.
|
||||
* Computes a raised to the power of b modular n
|
||||
*/
|
||||
inline int modular_exponentiation(int a,int b,int n)
|
||||
{
|
||||
int d = 1;
|
||||
while(b!=0)
|
||||
{
|
||||
if((b&1)!=0)
|
||||
d=(d*a)%n;
|
||||
a = (a*a)%n;
|
||||
b>>=1;
|
||||
}
|
||||
return d;
|
||||
}
|
||||
|
||||
/**
|
||||
* Prime Test.
|
||||
* This method tests if the specified integer is a prime.
|
||||
*
|
||||
* Extremely fast, does not allways find
|
||||
* a correct solution. However it fails
|
||||
* rarely and it only makes error of one
|
||||
* type.
|
||||
*
|
||||
* Some base-2 pseudo primes are 341, 561,645 and 1105.
|
||||
*
|
||||
* @param n The number to test.
|
||||
* @return If n was prime then the return value is
|
||||
* true otherwise it is false.
|
||||
*/
|
||||
inline bool pseudo_prime(int n)
|
||||
{
|
||||
if((n==1)||(n==2))//--- trivial cases
|
||||
return true;
|
||||
if(modular_exponentiation(2,n-1,n)!=1)
|
||||
return false;//--- definitely not a prime, i.e. composite
|
||||
return true;//--- possible prime or a base-2 pseudoprime
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Closest Prime Search.
|
||||
* This method tries to find the closest
|
||||
* prime number to n.
|
||||
*
|
||||
* @param n The seed number from where the
|
||||
* search should be done from.
|
||||
* @return The closest prime number to n.
|
||||
*/
|
||||
inline int prime_search(int n)
|
||||
{
|
||||
int l = n;
|
||||
if((l%2)==0)
|
||||
l--;
|
||||
for(;l>1;l=l-2)
|
||||
{
|
||||
// if(trial_division(l))
|
||||
// if(pseudo_prime(l))
|
||||
if(miller_rabin(l,4))
|
||||
break;
|
||||
}
|
||||
int o = n;
|
||||
if((o%2)==0)
|
||||
o++;
|
||||
int max_val = detail::highest<int>();
|
||||
for(;o<max_val;o=o+2)
|
||||
{
|
||||
// if(trial_division(o))
|
||||
// if(pseudo_prime(o))
|
||||
if(miller_rabin(o,4))
|
||||
break;
|
||||
}
|
||||
//--- now l and o must contain two closest
|
||||
//--- primes to n such that l <= n <= o
|
||||
//--- figure out which one that is closest
|
||||
//--- to n and return it.
|
||||
if((n-l)<(o-n))
|
||||
return l;
|
||||
return o;
|
||||
}
|
||||
|
||||
/**
|
||||
* Greatest Common Divisor.
|
||||
* Computes greatest common divisor with euclids algorithm.
|
||||
*
|
||||
* @param a
|
||||
* @param b
|
||||
* @return Greatest common divisor between a and b.
|
||||
*/
|
||||
inline int gcd_euclid_algorithm(int a,int b)
|
||||
{
|
||||
if(b==0)
|
||||
return a;
|
||||
else
|
||||
return gcd_euclid_algorithm(b,a%b);
|
||||
}
|
||||
|
||||
/**
|
||||
* Tests if two numbers are relative prime.
|
||||
*
|
||||
* @return If a and b are relative prime then the
|
||||
* return value is true otherwise it is
|
||||
* false.
|
||||
*/
|
||||
inline bool is_relative_prime(int a,int b)
|
||||
{
|
||||
if(gcd_euclid_algorithm(a,b)==1)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_MATH_PRIME_NUMBERS_H
|
||||
#endif
|
||||
@@ -0,0 +1,847 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_QUATERNION_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_QUATERNION_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <OpenTissue/core/math/math_vector3.h>
|
||||
#include <OpenTissue/core/math/math_matrix3x3.h>
|
||||
#include <OpenTissue/core/math/math_value_traits.h>
|
||||
#include <OpenTissue/core/math/math_is_number.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <iosfwd>
|
||||
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template<
|
||||
typename value_type_
|
||||
//, typename value_traits_ = ValueTraits<value_type_>
|
||||
>
|
||||
class Quaternion
|
||||
{
|
||||
protected:
|
||||
|
||||
typedef typename OpenTissue::math::ValueTraits<value_type_> value_traits_ ; // TODO value_traits_ should be parameterized as a class template parameter.
|
||||
|
||||
public:
|
||||
|
||||
typedef value_traits_ value_traits; ///< Convenience typedef to make value traits accessible for all template functions using Vector3 types.
|
||||
typedef value_type_ value_type; ///< Typedef is required for compliance with many other libraries and data containers!
|
||||
typedef Vector3<value_type> vector3_type; // TODO should Vector3 type be template parameterized?
|
||||
typedef Matrix3x3<value_type> matrix3x3_type;
|
||||
typedef size_t index_type; // TODO should the index type be template parameterized?
|
||||
|
||||
protected:
|
||||
|
||||
value_type m_s; ///< The real part.
|
||||
vector3_type m_v; ///< The R3 part or imaginary part.
|
||||
|
||||
public:
|
||||
|
||||
value_type & s() { return m_s; }
|
||||
value_type const & s() const { return m_s; }
|
||||
|
||||
vector3_type & v() { return m_v; }
|
||||
vector3_type const & v() const { return m_v; }
|
||||
|
||||
public:
|
||||
|
||||
Quaternion()
|
||||
: m_s(value_traits::one())
|
||||
, m_v(value_traits::zero(),value_traits::zero(),value_traits::zero())
|
||||
{}
|
||||
|
||||
~Quaternion(){}
|
||||
|
||||
Quaternion(Quaternion const & q) { *this = q; }
|
||||
|
||||
explicit Quaternion(matrix3x3_type const & M){ *this = M; }
|
||||
|
||||
explicit Quaternion(value_type const & s_val, value_type const & x, value_type const & y, value_type const & z)
|
||||
: m_s(s_val)
|
||||
, m_v(x,y,z)
|
||||
{ }
|
||||
|
||||
explicit Quaternion(value_type const & s_val, vector3_type const & v_val)
|
||||
: m_s(s_val)
|
||||
, m_v(v_val)
|
||||
{ }
|
||||
|
||||
Quaternion & operator=(Quaternion const & cpy)
|
||||
{
|
||||
m_s = cpy.m_s;
|
||||
m_v = cpy.m_v;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Assignment operator.
|
||||
* This method is a little special. Quaternions can be used to represent rotations, so can matrices.
|
||||
* This method transforms a rotation matrix into a Quaternion and then it assigns it to this Quaternion.
|
||||
* In short this method converts matrices into quaternions.
|
||||
*
|
||||
* @param M A reference to a matrix. This matrix should be a rotation matrix. That is an orthogonal matrix.
|
||||
*/
|
||||
Quaternion & operator=(matrix3x3_type const & M)
|
||||
{
|
||||
using std::sqrt;
|
||||
|
||||
value_type const & M00 = M(0,0);
|
||||
value_type const & M01 = M(0,1);
|
||||
value_type const & M02 = M(0,2);
|
||||
value_type const & M10 = M(1,0);
|
||||
value_type const & M11 = M(1,1);
|
||||
value_type const & M12 = M(1,2);
|
||||
value_type const & M20 = M(2,0);
|
||||
value_type const & M21 = M(2,1);
|
||||
value_type const & M22 = M(2,2);
|
||||
|
||||
value_type tr = M00 + M11 + M22;
|
||||
value_type r;
|
||||
|
||||
value_type const half = value_traits::one()/value_traits::two();
|
||||
|
||||
if(tr>=value_traits::zero())
|
||||
{
|
||||
r = sqrt(tr + value_traits::one());
|
||||
m_s = half*r;
|
||||
r = half/r;
|
||||
m_v[0] = (M21 - M12) * r;
|
||||
m_v[1] = (M02 - M20) * r;
|
||||
m_v[2] = (M10 - M01) * r;
|
||||
}
|
||||
else
|
||||
{
|
||||
int i = 0;
|
||||
if(M11>M00)
|
||||
i = 1;
|
||||
if(M22>M(i,i))
|
||||
i = 2;
|
||||
switch(i)
|
||||
{
|
||||
case 0:
|
||||
r = sqrt((M00 - (M11+M22)) + value_traits::one());
|
||||
m_v[0] = half*r;
|
||||
r = half/r;
|
||||
m_v[1] = (M01 + M10) * r;
|
||||
m_v[2] = (M20 + M02) * r;
|
||||
m_s = (M21 - M12) * r;
|
||||
break;
|
||||
case 1:
|
||||
r = sqrt((M11 - (M22+M00)) + value_traits::one());
|
||||
m_v[1] = half*r;
|
||||
r = half/r;
|
||||
m_v[2] = (M12 + M21)*r;
|
||||
m_v[0] = (M01 + M10)*r;
|
||||
m_s = (M02 - M20)*r;
|
||||
break;
|
||||
case 2:
|
||||
r = sqrt((M22 - (M00+M11)) + value_traits::one());
|
||||
m_v[2] = half*r;
|
||||
r = half/r;
|
||||
m_v[0] = (M20 + M02) * r;
|
||||
m_v[1] = (M12 + M21) * r;
|
||||
m_s = (M10 - M01) * r;
|
||||
break;
|
||||
};
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
// TODO: Comparing floats with == or != is not safe NOTE value_type might not be a float type it could be anything? This suggest that we need some kind of metaprogramming technique to deal with this problem?
|
||||
bool operator==(Quaternion const & cmp) const { return m_s==cmp.m_s && m_v==cmp.m_v; }
|
||||
bool operator!=(Quaternion const & cmp) const { return !(*this==cmp); }
|
||||
|
||||
public:
|
||||
|
||||
Quaternion & operator+= (Quaternion const & q )
|
||||
{
|
||||
m_s += q.m_s;
|
||||
m_v += q.m_v;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Quaternion & operator-= (Quaternion const & q )
|
||||
{
|
||||
m_s -= q.m_s;
|
||||
m_v -= q.m_v;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Quaternion operator+ ( Quaternion const &q ) const { return Quaternion(m_s + q.m_s, m_v + q.m_v); }
|
||||
Quaternion operator- ( Quaternion const &q ) const { return Quaternion(m_s - q.m_s, m_v - q.m_v); }
|
||||
|
||||
Quaternion & operator*=(value_type const &s_val )
|
||||
{
|
||||
m_s *= s_val;
|
||||
m_v *= s_val;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Quaternion & operator/=( value_type const &s_val )
|
||||
{
|
||||
assert(s_val || !"Quaternion::operator/=(): division by zero");
|
||||
m_s /= s_val;
|
||||
m_v /= s_val;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Quaternion operator-() const { return Quaternion(-m_s,-m_v); }
|
||||
|
||||
value_type operator*(Quaternion const &q) const { return m_s*q.m_s + m_v*q.m_v; }
|
||||
|
||||
/**
|
||||
* Multiplication of quaternions.
|
||||
* This method multiplies two quaternions with each other
|
||||
* and assigns the result to this Quaternion.
|
||||
*
|
||||
* @param b The second Quaternion.
|
||||
*/
|
||||
Quaternion operator%(Quaternion const & b)
|
||||
{
|
||||
return Quaternion( m_s*b.s() - dot(m_v , b.v()), cross(m_v , b.v()) + b.v()*m_s + m_v*b.s() );
|
||||
}
|
||||
|
||||
/**
|
||||
* Quaternion Vector Multiplication.
|
||||
*
|
||||
* @param v A Quaternion as a vector, i.e. zero scalar value.
|
||||
*/
|
||||
Quaternion operator%(vector3_type const & v_val)
|
||||
{
|
||||
return Quaternion( - dot( m_v() , v_val) , cross(m_v , v_val) + v_val*m_s );
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* Assigns the quaternion to the identity rotation.
|
||||
* This is a commonly used constant. It corresponds to the identity matrix.
|
||||
*/
|
||||
void identity()
|
||||
{
|
||||
m_s = value_traits::one();
|
||||
m_v.clear();
|
||||
}
|
||||
|
||||
/**
|
||||
* This method constructs a unit Quaternion which represents the specified rotation around the x-axis.
|
||||
*
|
||||
* @param rad The rotation angle in radians around the axis.
|
||||
*/
|
||||
void Rx(value_type const & rad)
|
||||
{
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
value_type teta = rad/value_traits::two();
|
||||
value_type cteta = (value_type)( cos(teta) );
|
||||
value_type steta = (value_type)( sin(teta) );
|
||||
|
||||
m_s = cteta;
|
||||
m_v(0) = steta;
|
||||
m_v(1) = value_traits::zero();
|
||||
m_v(2) = value_traits::zero();
|
||||
}
|
||||
|
||||
/**
|
||||
* This method constructs a unit Quaternion which represents the specified rotation around the y-axis.
|
||||
*
|
||||
* @param rad The rotation angle in radians around the axis.
|
||||
*/
|
||||
void Ry(value_type const & rad)
|
||||
{
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
value_type teta = rad/value_traits::two();
|
||||
value_type cteta = (value_type)( cos(teta) );
|
||||
value_type steta = (value_type)( sin(teta) );
|
||||
|
||||
m_s = cteta;
|
||||
m_v(0) = value_traits::zero();
|
||||
m_v(1) = steta;
|
||||
m_v(2) = value_traits::zero();
|
||||
}
|
||||
|
||||
/**
|
||||
* This method constructs a unit Quaternion which represents the specified rotation around the z-axis.
|
||||
*
|
||||
* @param rad The rotation angle in radians around the axis.
|
||||
*/
|
||||
void Rz(value_type const & rad)
|
||||
{
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
value_type teta = rad/value_traits::two();
|
||||
value_type cteta = (value_type)( cos(teta) );
|
||||
value_type steta = (value_type)( sin(teta) );
|
||||
|
||||
m_s = cteta;
|
||||
m_v(0) = value_traits::zero();
|
||||
m_v(1) = value_traits::zero();
|
||||
m_v(2) = steta;
|
||||
}
|
||||
|
||||
/**
|
||||
* Rotate Vector by Quaternion.
|
||||
*
|
||||
* computes r' = q*r*conj(q)
|
||||
*/
|
||||
vector3_type rotate(vector3_type const & r) const { return ((*this) % r % conj(*this)).v(); }
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* Equality comparison with an error bound.
|
||||
* The test is exactly the same as with the method without error bound the only difference
|
||||
* is that the corresponding terms of the quaternions are said to be equal if they do not
|
||||
* differ by more than the error bound value.
|
||||
*
|
||||
* @param q A reference to a Quaternion. This is the
|
||||
* Quaternion to compare with.
|
||||
* @param threshold A reference to the acceptable error bound.
|
||||
*
|
||||
* @return The test result. The return value is one if
|
||||
* the quaternions are equal otherwise the return
|
||||
* value is zero.
|
||||
*/
|
||||
bool is_equal(Quaternion const & q,value_type const & threshold) const
|
||||
{
|
||||
using std::fabs;
|
||||
|
||||
assert( threshold>=value_traits::zero() || !"is_equal(): threshold must be non-negative");
|
||||
|
||||
return fabs(m_s-q.m_s)<threshold && m_v.is_equal(q.v(),threshold);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
template<typename T>
|
||||
inline Vector3<T> rotate(Quaternion<T> const & q, Vector3<T> const & r)
|
||||
{
|
||||
return prod( prod(q , r) , conj(q)).v();
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Quaternion<T> prod(Quaternion<T> const & a, Quaternion<T> const & b)
|
||||
{
|
||||
return Quaternion<T>( a.s()*b.s() - dot(a.v() , b.v()), cross(a.v() , b.v()) + b.v()*a.s() + a.v()*b.s() );
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Quaternion<T> prod(Quaternion<T> const & a, Vector3<T> const & b)
|
||||
{
|
||||
return Quaternion<T>( - dot(a.v() , b), cross(a.v() , b) + b*a.s() );
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Quaternion<T> prod(Vector3<T> const & a, Quaternion<T> const & b)
|
||||
{
|
||||
return Quaternion<T>( - dot(a , b.v()), cross(a , b.v()) + a*b.s() );
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Quaternion<T> operator%(Quaternion<T> const & a, Quaternion<T> const & b) { return prod(a,b); }
|
||||
template<typename T>
|
||||
inline Quaternion<T> operator%(Quaternion<T> const & a, Vector3<T> const & b) { return prod(a,b); }
|
||||
template<typename T>
|
||||
inline Quaternion<T> operator%(Vector3<T> const & a, Quaternion<T> const & b) { return prod(a,b); }
|
||||
template <typename T, typename T2>
|
||||
inline Quaternion<T> operator*( const Quaternion<T> &q, const T2 &s_val ) { return Quaternion<T>( q.s()*s_val, q.v()*s_val); }
|
||||
template <typename T2, typename T>
|
||||
inline Quaternion<T> operator*( const T2 &s_val, const Quaternion<T> &q ) { return Quaternion<T>( q.s()*s_val, q.v()*s_val); }
|
||||
template <typename T, typename T2>
|
||||
inline Quaternion<T> operator/( const Quaternion<T> &q, const T2 &s_val ) { return Quaternion<T>( q.s()/s_val, q.v()/s_val); }
|
||||
template <typename T2, typename T>
|
||||
inline Quaternion<T> operator/( const T2 &s_val, const Quaternion<T> &q ) { return Quaternion<T>( q.s()/s_val, q.v()/s_val); }
|
||||
|
||||
template<typename T>
|
||||
inline T const length(Quaternion<T> const & q)
|
||||
{
|
||||
using std::sqrt;
|
||||
return sqrt( q*q );
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Quaternion<T> unit(Quaternion<T> const & q)
|
||||
{
|
||||
typedef typename Quaternion<T>::value_traits value_traits;
|
||||
|
||||
using std::sqrt;
|
||||
using std::fabs;
|
||||
|
||||
T l = length(q);
|
||||
|
||||
if(fabs(l) > value_traits::zero())
|
||||
return Quaternion<T> (q.s()/l,q.v()/l);
|
||||
return Quaternion<T> (value_traits::zero(),value_traits::zero(),value_traits::zero(),value_traits::zero());
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Quaternion<T> normalize(Quaternion<T> const & q) { return unit(q); }
|
||||
|
||||
/**
|
||||
* Natural Logarithm
|
||||
* Returns the Quaternion equal to the natural logarithm of
|
||||
* the specified Quaternion.
|
||||
*
|
||||
* @param q A reference to an unit quaternion.
|
||||
* @return
|
||||
*/
|
||||
template<typename T>
|
||||
inline Quaternion<T> log(Quaternion<T> const & q)
|
||||
{
|
||||
typedef typename Quaternion<T>::value_traits value_traits;
|
||||
|
||||
using std::acos;
|
||||
using std::sin;
|
||||
|
||||
if(q.s()==value_traits::one() && is_zero(q.v()))
|
||||
return Quaternion<T>(value_traits::zero(),value_traits::zero(),value_traits::zero(),value_traits::zero());
|
||||
|
||||
T teta = (T)( acos(q.s()) );
|
||||
T st = (T)( sin(teta) );
|
||||
return Quaternion<T>( value_traits::zero(), q.v()*(teta/st) );
|
||||
}
|
||||
|
||||
/**
|
||||
* Orthogonal Quaternion.
|
||||
* This method sets this Quaternion to an orthogonal Quaternion
|
||||
* of the specified Quaternion. In other words the resulting
|
||||
* angle between the specified Quaternion and this Quaternion
|
||||
* is pi/2.
|
||||
*/
|
||||
template<typename T>
|
||||
inline Quaternion<T> hat(Quaternion<T> const & q)
|
||||
{
|
||||
return Quaternion<T>( q.v()(2), - q.v()(1) , q.v()(0), -q.s());
|
||||
}
|
||||
|
||||
/**
|
||||
* Exponent
|
||||
* Sets the Quaternion equal to the exponent of
|
||||
* the specified Quaternion.
|
||||
*
|
||||
* @param q A reference to a pure Quaternion (zero
|
||||
* T part).
|
||||
*/
|
||||
template<typename T>
|
||||
inline Quaternion<T> exp(Quaternion<T> const & q)
|
||||
{
|
||||
using std::sqrt;
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
//--- teta^2 x^2 + teta^2 y^2 +teta^2 z^2 =
|
||||
//--- teta^2 (x^2 + y^2 + z^2) = teta^2
|
||||
T teta = (T)( sqrt(q.v() *q.v()) );
|
||||
T ct = (T)( cos(teta) );
|
||||
T st = (T)( sin(teta) );
|
||||
|
||||
return Quaternion<T>(ct,q.v()*st);
|
||||
}
|
||||
|
||||
/**
|
||||
* QLERP - Linear Interpolation of Quaterions.
|
||||
*
|
||||
* @param A Quaternion A
|
||||
* @param B Quaternion B
|
||||
* @param w The weight
|
||||
*
|
||||
* @return The resulting Quaternion, (1-w)A+w B. Note the resulting
|
||||
* Quaternion may not be a unit quaternion even though A and B are
|
||||
* unit quaternions. If a unit Quaternion is needed write
|
||||
* unit(qlerp(A,B,w)).
|
||||
*
|
||||
* -Added by spreak for the SBS algorithm, see OpenTissue/kinematics/skinning/SBS
|
||||
*/
|
||||
template<typename T>
|
||||
// TODO why not simply call this function lerp?
|
||||
inline Quaternion<T> qlerp(Quaternion<T> const & A,Quaternion<T> const & B,T const & w)
|
||||
{
|
||||
typedef typename Quaternion<T>::value_traits value_traits;
|
||||
|
||||
assert(w>=value_traits::zero() || !"qlerp(): w must not be less than 0");
|
||||
assert(w<=value_traits::one() || !"qlerp(): w must not be larger than 1");
|
||||
T mw = value_traits::one() - w;
|
||||
return ((mw * A) + (w * B));
|
||||
}
|
||||
|
||||
/**
|
||||
* Spherical Linear Interpolation of Quaternions.
|
||||
*
|
||||
* @param A
|
||||
* @param B
|
||||
* @param w The weight
|
||||
*
|
||||
* @return The resulting Quaternion.
|
||||
*/
|
||||
template<typename T>
|
||||
inline Quaternion<T> slerp(Quaternion<T> const & A,Quaternion<T> const & B,T const & w)
|
||||
{
|
||||
typedef typename Quaternion<T>::value_traits value_traits;
|
||||
|
||||
using std::acos;
|
||||
using std::sin;
|
||||
|
||||
assert(w>=value_traits::zero() || !"slerp(): w must not be less than 0");
|
||||
assert(w<=value_traits::one() || !"slerp(): w must not be larger than 1");
|
||||
|
||||
T q_tiny = (T)( 10e-7 ); // TODO prober constant type conversion?
|
||||
|
||||
T norm = A*B;
|
||||
|
||||
bool flip = false;
|
||||
if( norm < value_traits::zero() )
|
||||
{
|
||||
norm = -norm;
|
||||
flip = true;
|
||||
}
|
||||
T weight = w;
|
||||
T inv_weight;
|
||||
if(value_traits::one() - norm < q_tiny)
|
||||
{
|
||||
inv_weight = value_traits::one() - weight;
|
||||
}
|
||||
else
|
||||
{
|
||||
T theta = (T)( acos(norm) );
|
||||
T s_val = (T)( value_traits::one() / sin(theta) );
|
||||
inv_weight = (T)( sin((value_traits::one() - weight) * theta) * s_val );
|
||||
weight = (T)( sin(weight * theta) * s_val );
|
||||
}
|
||||
if(flip)
|
||||
{
|
||||
weight = -weight;
|
||||
}
|
||||
return ( inv_weight * A + weight * B);
|
||||
}
|
||||
|
||||
/**
|
||||
* "Cubical" Spherical Interpolation.
|
||||
* In popular terms this corresponds to a cubic spline in
|
||||
* ordinary 3D space. However it is really a series of
|
||||
* spherical linear interpolations, which defines a
|
||||
* cubic on the unit Quaternion sphere.
|
||||
*
|
||||
* @param q0
|
||||
* @param q1
|
||||
* @param q2
|
||||
* @param q3
|
||||
* @param u
|
||||
*
|
||||
* @return
|
||||
*/
|
||||
template<typename T>
|
||||
inline Quaternion<T> squad(
|
||||
Quaternion<T> const & q0
|
||||
, Quaternion<T> const & q1
|
||||
, Quaternion<T> const & q2
|
||||
, Quaternion<T> const & q3
|
||||
, T const & u
|
||||
)
|
||||
{
|
||||
typedef typename Quaternion<T>::value_traits value_traits;
|
||||
|
||||
assert(u>=value_traits::zero() || !"squad(): u must not be less than 0");
|
||||
assert(u<=value_traits::one() || !"squad(): u must not be larger than 1");
|
||||
|
||||
T u2 = value_traits::two() *u*(value_traits::one() -u);
|
||||
return slerp( slerp(q0,q3,u), slerp(q1,q2,u), u2);
|
||||
}
|
||||
|
||||
/**
|
||||
* Quaternion Conjugate.
|
||||
*/
|
||||
template<typename T>
|
||||
inline Quaternion<T> conj(Quaternion<T> const & q)
|
||||
{
|
||||
return Quaternion<T>(q.s(),-q.v());
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Get Axis Angle Representation.
|
||||
* This function converts a unit-quaternion into the
|
||||
* equivalent angle-axis representation.
|
||||
*
|
||||
* @param Q The quaternion
|
||||
* @param axis Upon return this argument holds value of the equivalent rotation axis.
|
||||
* @param theta Upon return this argument holds value of the equivalent rotation angle.
|
||||
*/
|
||||
template<typename T>
|
||||
inline void get_axis_angle(Quaternion<T> const & Q,Vector3<T> & axis, T & theta)
|
||||
{
|
||||
using std::atan2;
|
||||
|
||||
typedef typename Quaternion<T>::value_traits value_traits;
|
||||
typedef Vector3<T> V;
|
||||
|
||||
//
|
||||
// By definition a unit quaternion Q can be written as
|
||||
//
|
||||
// Q = [s,v] = [cos(theta/2), n sin(theta/2)]
|
||||
//
|
||||
// where n is a unit vector. This is the same as a rotation of
|
||||
// theta radian around the axis n.
|
||||
//
|
||||
//
|
||||
// Rotations are difficult to work with for several reasons.
|
||||
//
|
||||
// Firstly both Q and -Q represent the same rotation. This is
|
||||
// easily proven, rotate a arbitrary vector r by Q then we have
|
||||
//
|
||||
// r^\prime = Q r Q^*
|
||||
//
|
||||
// Now rotate the same vector by -Q
|
||||
//
|
||||
// r^\prime = (-Q) r (-Q)^* = Q r Q^*
|
||||
//
|
||||
// because -Q = [-s,-v] and (-Q)^* = [-s , v] = - [s,-v]^* = - Q^*.
|
||||
//
|
||||
// Thus the quaternion representation of a single rotation is not unique.
|
||||
//
|
||||
// Secondly the rotation it self is not well-posed. A rotation of theta
|
||||
// radians around the unit axis n could equally well be done as a rotation
|
||||
// of -theta radians around the negative unit axis n.
|
||||
//
|
||||
// This is seen by straightforward substitution
|
||||
//
|
||||
// [ cos(-theta/2), sin(-theta/2) (-n) ] = [ cos(theta/2), sin(theta/2) n ]
|
||||
//
|
||||
// Thus we get the same quaternion regardless of whether we
|
||||
// use (+theta,+n) or (-theta,-n).
|
||||
//
|
||||
//
|
||||
// From the Quaternion we see that
|
||||
//
|
||||
// \frac{v}{\norm{v}} = \frac{ sin(theta/2) n }{| sin(theta/2) | } = sign(sin(theta/2)) n
|
||||
//
|
||||
// Thus we can easily get the rotation axis. However, we can not immediately
|
||||
// determine the positive rotation axis direction. The problem boils down to the
|
||||
// fact that we can not see the sign of the sinus-factor.
|
||||
//
|
||||
// Let us proceed by setting
|
||||
//
|
||||
// x = cos(theta/2) = s
|
||||
// y = | sin(theta/2) | = \norm{v}
|
||||
//
|
||||
// Then we basically have two possibilities for finding theta
|
||||
//
|
||||
// theta_1 = 2 atan2( y, x) equivalent to sign(sin(theta/2)) = 1
|
||||
//
|
||||
// or
|
||||
//
|
||||
// theta_2 = 2 atan2( -y, x) equivalent to sign(sin(theta/2)) = -1
|
||||
//
|
||||
// If theta_1 is the solution we have
|
||||
//
|
||||
// n = \frac{v}{\norm{v}}
|
||||
//
|
||||
// If theta_2 is the solution we must have
|
||||
//
|
||||
// n = - \frac{v}{\norm{v}}
|
||||
//
|
||||
// Observe that we always have theta_2 = 2 pi - theta_1. Therefore theta_1 < theta_2.
|
||||
//
|
||||
// Let us imagine that we always choose $theta_1$ as the solution then
|
||||
// the corresponding quaternion for that solution would be
|
||||
//
|
||||
//
|
||||
// Q_1 = [cos(theta_1/2), sin(theta_1/2) \frac{v}{\norm{v}}]
|
||||
// = [s , \norm{v} \frac{v}{\norm{v}}]
|
||||
// = Q
|
||||
//
|
||||
// Now if we choose theta_2 as the solution we would have
|
||||
//
|
||||
// Q_2 = [cos(theta_2/2), sin(theta_2/2) -\frac{v}{\norm{v}}]
|
||||
// = [s , -\norm{v} -\frac{v}{\norm{v}}]
|
||||
// = [s , \norm{v} \frac{v}{\norm{v}}]
|
||||
// = Q
|
||||
//
|
||||
// Thus we observe that regardless of which solution we pick we always have Q = Q_1 = Q_2.
|
||||
//
|
||||
// At this point one may be confused. However, it should be clear that theta_2 is equivalent
|
||||
// to the theta_1 rotation. The difference is simply that theta_2 corresponds to flipping the
|
||||
// rotation axis of the theta_1 case.
|
||||
//
|
||||
T const ct2 = Q.s(); //--- cos(theta/2)
|
||||
T const st2 = length( Q.v() ); //--- |sin(theta/2)|
|
||||
|
||||
theta = value_traits::two()* atan2(st2,ct2);
|
||||
|
||||
assert( st2 >= value_traits::zero() || !"get_axis_angle(): |sin(theta/2)| must be non-negative");
|
||||
assert( theta >= value_traits::zero() || !"get_axis_angle(): theta must be non-negative");
|
||||
assert( is_number(theta) || !"get_axis_angle(): NaN encountered");
|
||||
|
||||
axis = st2 > value_traits::zero() ? Q.v() / st2 : V(value_traits::zero(), value_traits::zero(), value_traits::zero());
|
||||
}
|
||||
|
||||
/**
|
||||
* Get Rotation Angle wrt. an Axis.
|
||||
* Think of it as if you have a fixated body A so only body B is allowed to move.
|
||||
*
|
||||
* Let BF_B' indicate the initial orientation of body B's body frame
|
||||
* and BF_B the current orientation, now BF_A should be thought of as
|
||||
* being immovable ie. constant.
|
||||
*
|
||||
* Q_initial : BF_A -> BF_B'
|
||||
* Q_cur : BF_A -> BF_B
|
||||
*
|
||||
* Now we must have the relation
|
||||
*
|
||||
* Q_rel Q_initial = Q_cur
|
||||
*
|
||||
* From which we deduce
|
||||
*
|
||||
* Q_rel = Q_cur conj(Q_initial)
|
||||
*
|
||||
* And we see that
|
||||
*
|
||||
* Q_rel : BF_B' -> BF_B
|
||||
*
|
||||
* That is how much the body frame of body B have rotated (measured
|
||||
* with respect to the fixed body frame A).
|
||||
*
|
||||
*
|
||||
* @param Q_rel Rotation from initial orientation of B to current orientation of B.
|
||||
* @param axis The rotation axis in the body frame of body B.
|
||||
*
|
||||
* @return The angle in radians.
|
||||
*/
|
||||
template<typename T>
|
||||
inline T get_angle(Quaternion<T> const & Q_rel,Vector3<T> const & axis)
|
||||
{
|
||||
typedef typename Quaternion<T>::value_traits value_traits;
|
||||
|
||||
using std::atan2;
|
||||
|
||||
//--- The angle between the two bodies is extracted from the Quaternion Q_rel
|
||||
//---
|
||||
//--- [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
|
||||
//---
|
||||
//--- where s is a value_type and v is a 3-vector. u is a unit length axis and
|
||||
//--- theta is a rotation along that axis.
|
||||
//---
|
||||
//--- we can get theta/2 by:
|
||||
//---
|
||||
//--- theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
|
||||
//---
|
||||
//--- but we can not get sin(theta/2) directly, only its absolute value:
|
||||
//---
|
||||
//--- |v| = |sin(theta/2)| * |u| = |sin(theta/2)|
|
||||
//---
|
||||
//--- using this value will have a strange effect.
|
||||
//---
|
||||
//--- Recall that there are two Quaternion representations of a given
|
||||
//--- rotation, q and -q.
|
||||
//---
|
||||
//--- Typically as a body rotates along the axis it will go through a
|
||||
//--- complete cycle using one representation and then the next cycle
|
||||
//--- will use the other representation.
|
||||
//---
|
||||
//--- This corresponds to u pointing in the direction of the joint axis and
|
||||
//--- then in the opposite direction. The result is that theta
|
||||
//--- will appear to go "backwards" every other cycle.
|
||||
//---
|
||||
//--- Here is a fix: if u points "away" from the direction of the joint
|
||||
//--- axis (i.e. more than 90 degrees) then use -q instead of q. This
|
||||
//--- represents the same rotation, but results in the cos(theta/2) value
|
||||
//--- being sign inverted.
|
||||
T ct2 = Q_rel.s(); //--- cos(theta/2)
|
||||
T st2 = length( Q_rel.v() ); //--- |sin(theta/2)|
|
||||
T theta = value_traits::zero();
|
||||
|
||||
//--- Remember that Q_rel : BF_B' -> BF_B, so we need the axis in body B's local frame
|
||||
if( Q_rel.v() * axis >= value_traits::zero())
|
||||
{
|
||||
//--- u points in direction of axis.
|
||||
//std::cout << "u points in direction of axis" << std::endl;
|
||||
theta = value_traits::two()* atan2(st2,ct2);
|
||||
}
|
||||
else
|
||||
{
|
||||
//--- u points in opposite direction.
|
||||
//std::cout << "u points in opposite direction" << std::endl;
|
||||
theta = value_traits::two() * atan2(st2,-ct2);
|
||||
}
|
||||
//--- The angle we get will be between 0..2*pi, but we want
|
||||
//--- to return angles between -pi..pi
|
||||
if (theta > value_traits::pi())
|
||||
theta -= value_traits::two()*value_traits::pi();
|
||||
|
||||
//--- The angle we've just extracted has the wrong sign (Why???).
|
||||
theta = -theta;
|
||||
//
|
||||
// Say we have a rotation, R, relating the coordinates of two frames X and Y, now
|
||||
// let the coordinates of the vector v be given in frame X as
|
||||
//
|
||||
// [v]_X
|
||||
//
|
||||
// And in frame Y as
|
||||
//
|
||||
// [v]_Y
|
||||
//
|
||||
// Now R is defined such that
|
||||
//
|
||||
// R [v]_X = [v]_Y
|
||||
//
|
||||
// That is it changes the coordinates of v from X to Y.
|
||||
//
|
||||
// This is pretty straightforward, but there is some subtlety in it, say
|
||||
// frame Y is rotated theta radians around the z-axis, then the rotation
|
||||
// matrix relating the coordinates is the opposite rotation.
|
||||
//
|
||||
// What we want to measure is how much the frame axis have rotated, but
|
||||
// what we are given is a rotation transforming coordinates, therefore
|
||||
// we need to flip the sign of the extracted angle!!!
|
||||
return theta;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline std::ostream & operator<< (std::ostream & o,Quaternion<T> const & q)
|
||||
{
|
||||
o << "[" << q.s() << "," << q.v()(0) << "," << q.v()(1) << "," << q.v()(2) << "]";
|
||||
return o;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline std::istream & operator>>(std::istream & i,Quaternion<T> & q)
|
||||
{
|
||||
char dummy;
|
||||
|
||||
i >> dummy;
|
||||
i >> q.s();
|
||||
i >> dummy;
|
||||
i >> q.v()(0);
|
||||
i >> dummy;
|
||||
i >> q.v()(1);
|
||||
i >> dummy;
|
||||
i >> q.v()(2);
|
||||
i >> dummy;
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_MATH_QUATERNION_H
|
||||
#endif
|
||||
@@ -0,0 +1,48 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_VALUE_TRAITS_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_VALUE_TRAITS_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <OpenTissue/core/math/math_constants.h>
|
||||
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
class ValueTraits
|
||||
{
|
||||
public:
|
||||
|
||||
static T zero() { return detail::zero<T>(); }
|
||||
static T one() { return detail::one<T>(); }
|
||||
static T two() { return detail::two<T>(); }
|
||||
static T three() { return detail::three<T>(); }
|
||||
static T four() { return detail::four<T>(); }
|
||||
static T eight() { return detail::eight<T>(); }
|
||||
static T half() { return detail::half<T>(); }
|
||||
static T pi() { return detail::pi<T>(); }
|
||||
static T pi_2() { return detail::pi_half<T>(); }
|
||||
static T pi_half() { return detail::pi_half<T>(); }
|
||||
static T pi_quarter(){ return detail::pi_quarter<T>();}
|
||||
static T pi_4() { return detail::pi_quarter<T>();}
|
||||
static T degree() { return detail::degree<T>(); }
|
||||
static T radian() { return detail::radian<T>(); }
|
||||
|
||||
};
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_MATH_VALUE_TRAITS_H
|
||||
#endif
|
||||
577
Demos3/FiniteElementMethod/OpenTissue/core/math/math_vector3.h
Normal file
577
Demos3/FiniteElementMethod/OpenTissue/core/math/math_vector3.h
Normal file
@@ -0,0 +1,577 @@
|
||||
#ifndef OPENTISSUE_CORE_MATH_MATH_VECTORX3_H
|
||||
#define OPENTISSUE_CORE_MATH_MATH_VECTORX3_H
|
||||
//
|
||||
// OpenTissue Template Library
|
||||
// - A generic toolbox for physics-based modeling and simulation.
|
||||
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
|
||||
//
|
||||
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
|
||||
//
|
||||
#include <OpenTissue/configuration.h>
|
||||
|
||||
#include <OpenTissue/core/math/math_constants.h>
|
||||
#include <OpenTissue/core/math/math_functions.h>
|
||||
#include <OpenTissue/core/math/math_value_traits.h>
|
||||
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <cassert>
|
||||
|
||||
// TODO 2007-06-05 hod - Could not get this to work on linux.
|
||||
// Might have somthing to do with http://gcc.gnu.org/faq.html#friend
|
||||
//#include <iosfwd>
|
||||
#include <iostream>
|
||||
|
||||
|
||||
namespace OpenTissue
|
||||
{
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template <
|
||||
typename value_type_
|
||||
//, typename value_traits_ = ValueTraits<value_type_>
|
||||
>
|
||||
class Vector3
|
||||
{
|
||||
protected:
|
||||
|
||||
typedef typename OpenTissue::math::ValueTraits<value_type_> value_traits_ ; // TODO value_traits_ should be parameterized as a class template parameter.
|
||||
|
||||
public:
|
||||
|
||||
typedef value_traits_ value_traits; ///< Convience typedef to make value traits accesible for all template functions using Vector3 types.
|
||||
typedef value_type_ value_type; ///< Typedef is required for compliance with many other libraries and data containers!
|
||||
typedef size_t index_type; // TODO should the index type be template parameterized?
|
||||
|
||||
private:
|
||||
|
||||
// TODO 2007-02-17 KE: Refactor this stuff into an array
|
||||
|
||||
value_type x; ///< The first coordinate of this vector.
|
||||
value_type y; ///< The second coordinate of this vector.
|
||||
value_type z; ///< The third coordinate of this vector.
|
||||
|
||||
public:
|
||||
|
||||
|
||||
Vector3()
|
||||
: x( value_traits::zero() )
|
||||
, y( value_traits::zero() )
|
||||
, z( value_traits::zero() )
|
||||
{}
|
||||
|
||||
Vector3( Vector3 const & v )
|
||||
: x(v(0))
|
||||
, y(v(1))
|
||||
, z(v(2))
|
||||
{}
|
||||
|
||||
explicit Vector3( value_type const& val )
|
||||
: x( val )
|
||||
, y( val )
|
||||
, z( val )
|
||||
{}
|
||||
|
||||
template <typename T1,typename T2,typename T3>
|
||||
Vector3( T1 const & x_val, T2 const & y_val, T3 const & z_val )
|
||||
: x( (value_type)(x_val) )
|
||||
, y( (value_type)(y_val) )
|
||||
, z( (value_type)(z_val) )
|
||||
{}
|
||||
|
||||
~Vector3()
|
||||
{}
|
||||
|
||||
Vector3 & operator=(Vector3 const & cpy)
|
||||
{
|
||||
x=cpy(0);
|
||||
y=cpy(1);
|
||||
z=cpy(2);
|
||||
return *this;
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
void clear() { x = y = z = value_traits::zero(); }
|
||||
|
||||
size_t size() const { return 3u;}
|
||||
|
||||
public:
|
||||
|
||||
value_type & operator() ( index_type index )
|
||||
{
|
||||
assert( (index>=0 && index<3) || !"vecto3():index should be in range [0..2]");
|
||||
return *((&x) + index);
|
||||
}
|
||||
|
||||
value_type const & operator() ( index_type index ) const
|
||||
{
|
||||
assert( (index>=0 && index<3) || !"vecto3():index should be in range [0..2]");
|
||||
return *((&x) + index);
|
||||
}
|
||||
|
||||
value_type & operator[] ( index_type index )
|
||||
{
|
||||
assert( (index>=0 && index<3) || !"vecto3[]:index should be in range [0..2]");
|
||||
return *((&x) + index);
|
||||
}
|
||||
|
||||
value_type const & operator[] ( index_type index ) const
|
||||
{
|
||||
assert( (index>=0 && index<3) || !"vecto3[]:index should be in range [0..2]");
|
||||
return *((&x) + index);
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
bool operator< (Vector3 const & v) const
|
||||
{
|
||||
if ( x < v(0) ) return true;
|
||||
if ( x > v(0) ) return false;
|
||||
if ( y < v(1) ) return true;
|
||||
if ( y > v(1) ) return false;
|
||||
return z < v(2);
|
||||
}
|
||||
|
||||
bool operator> (Vector3 const & v) const
|
||||
{
|
||||
if ( x > v(0) ) return true;
|
||||
if ( x < v(0) ) return false;
|
||||
if ( y > v(1) ) return true;
|
||||
if ( y < v(1) ) return false;
|
||||
return z > v(2);
|
||||
}
|
||||
|
||||
// TODO: Comparing floats with == or != is not safe NOTE value_type might not be a float type it could be anything? This suggest that we need some kinf of metaprogramming technique to deal with ths problem?
|
||||
bool operator==(Vector3 const & cmp) const { return x==cmp.x && y==cmp.y && z==cmp.z; }
|
||||
bool operator!=(Vector3 const & cmp) const { return x!=cmp.x || y!=cmp.y || z!=cmp.z; }
|
||||
|
||||
public:
|
||||
|
||||
Vector3 & operator+= ( Vector3 const & v )
|
||||
{
|
||||
x += v(0);
|
||||
y += v(1);
|
||||
z += v(2);
|
||||
return *this;
|
||||
}
|
||||
|
||||
Vector3 & operator-= ( Vector3 const & v )
|
||||
{
|
||||
x -= v(0);
|
||||
y -= v(1);
|
||||
z -= v(2);
|
||||
return *this;
|
||||
}
|
||||
|
||||
Vector3 & operator*=( value_type const & s )
|
||||
{
|
||||
x *= s;
|
||||
y *= s;
|
||||
z *= s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Vector3 & operator/=( value_type const & s )
|
||||
{
|
||||
assert(s || !"Vector3::/=(): division by zero");
|
||||
x /= s;
|
||||
y /= s;
|
||||
z /= s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Vector3 operator+ ( Vector3 const & v ) const { return Vector3( x+v(0), y+v(1), z+v(2)); }
|
||||
Vector3 operator- ( Vector3 const & v ) const { return Vector3( x-v(0), y-v(1), z-v(2)); }
|
||||
Vector3 operator- ( ) const { return Vector3(-x,-y,-z); }
|
||||
Vector3 operator% ( Vector3 const & v ) const { return Vector3(y*v(2)-v(1)*z, v(0)*z-x*v(2), x*v(1)-v(0)*y); }
|
||||
value_type operator* ( Vector3 const & v ) const { return x*v(0) + y*v(1) + z*v(2); }
|
||||
bool operator<=( Vector3 const & v ) const { return x <= v(0) && y <= v(1) && z <= v(2); }
|
||||
bool operator>=( Vector3 const & v ) const { return x >= v(0) && y >= v(1) && z >= v(2); }
|
||||
|
||||
public:
|
||||
|
||||
friend Vector3 fabs( Vector3 const & v )
|
||||
{
|
||||
using std::fabs;
|
||||
return Vector3 (
|
||||
(value_type)( fabs( v(0) ) )
|
||||
, (value_type)( fabs( v(1) ) )
|
||||
, (value_type)( fabs( v(2) ) )
|
||||
);
|
||||
}
|
||||
|
||||
friend Vector3 min( Vector3 const & A, Vector3 const & B )
|
||||
{
|
||||
using std::min;
|
||||
return Vector3( min( A(0), B(0) ), min( A(1), B(1) ), min( A(2), B(2) ) );
|
||||
}
|
||||
|
||||
friend Vector3 max( Vector3 const & A, Vector3 const & B )
|
||||
{
|
||||
using std::max;
|
||||
return Vector3( max( A(0), B(0) ), max( A(1), B(1) ), max( A(2), B(2) ) );
|
||||
}
|
||||
|
||||
friend Vector3 floor(Vector3 const & v)
|
||||
{
|
||||
using std::floor;
|
||||
return Vector3(
|
||||
(value_type)( floor(v(0)) )
|
||||
, (value_type)( floor(v(1)) )
|
||||
, (value_type)( floor(v(2)) )
|
||||
);
|
||||
}
|
||||
|
||||
friend Vector3 ceil(Vector3 const & v)
|
||||
{
|
||||
using std::ceil;
|
||||
return Vector3(
|
||||
(value_type)( ceil(v(0)) )
|
||||
, (value_type)( ceil(v(1)) )
|
||||
, (value_type)( ceil(v(2)) )
|
||||
);
|
||||
}
|
||||
|
||||
friend std::ostream & operator<< (std::ostream & o,Vector3 const & v)
|
||||
{
|
||||
o << "[";
|
||||
o << v(0);
|
||||
o << ",";
|
||||
o << v(1);
|
||||
o << "," << v(2) << "]";
|
||||
return o;
|
||||
}
|
||||
|
||||
friend std::istream & operator>>(std::istream & i,Vector3 & v)
|
||||
{
|
||||
char dummy;
|
||||
i >> dummy;
|
||||
i >> v(0);
|
||||
i >> dummy;
|
||||
i >> v(1);
|
||||
i >> dummy;
|
||||
i >> v(2);
|
||||
i >> dummy;
|
||||
return i;
|
||||
}
|
||||
public:
|
||||
|
||||
// TODO 2007-02-17 KE: Kill this it should be handled with comparator traits in operator== and operator!=
|
||||
bool is_equal(Vector3 const & v, value_type const & threshold) const
|
||||
{
|
||||
using std::fabs;
|
||||
return fabs(x-v.x)<=threshold && fabs(y-v.y)<=threshold && fabs(z-v.z)<=threshold;
|
||||
}
|
||||
|
||||
}; // class Vector3
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
/// Declaration of vector3 non-member functions
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
template <typename T>
|
||||
inline Vector3<T> round(Vector3<T> const & v)
|
||||
{
|
||||
using std::floor;
|
||||
|
||||
static T const half = detail::half<T>();
|
||||
|
||||
return Vector3<T> (
|
||||
floor( v(0) + half )
|
||||
, floor( v(1) + half )
|
||||
, floor( v(2) + half )
|
||||
);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline typename Vector3<T>::index_type min_index(Vector3<T> const & v)
|
||||
{
|
||||
return v(0) <= v(1) && v(0) < v(2) ? 0 : v(1) <= v(0) && v(1) < v(2) ? 1 : 2;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline typename Vector3<T>::index_type max_index(Vector3<T> const & v)
|
||||
{
|
||||
return v(2) >= v(0) && v(2) >= v(1) ? 2 : v(1) >= v(0) && v(1) > v(2) ? 1 : 0;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline typename Vector3<T>::index_type mid_index(Vector3<T> const & v)
|
||||
{
|
||||
typename Vector3<T>::index_type test = min_index(v) + max_index(v);
|
||||
return test == 2 ? 1 : test == 1 ? 2 : 0;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline T min_value(Vector3<T> const & v)
|
||||
{
|
||||
using std::min;
|
||||
return min(v(0),min(v(1),v(2)));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline T max_value(Vector3<T> const & v)
|
||||
{
|
||||
using std::max;
|
||||
return max(v(0),max(v(1),v(2)));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline T mid_value(Vector3<T> const & v)
|
||||
{
|
||||
return v(mid_index(v));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline bool is_zero(Vector3<T> const & v, T const & threshold)
|
||||
{
|
||||
using std::fabs;
|
||||
return fabs(v(0))<=threshold && fabs(v(1))<=threshold && fabs(v(2))<=threshold;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline bool is_zero(Vector3<T> const & v)
|
||||
{
|
||||
typedef typename Vector3<T>::value_traits value_traits;
|
||||
return is_zero(v,value_traits::zero());
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
template <typename T>
|
||||
inline Vector3<T> cross( Vector3<T> const & a, Vector3<T> const & b )
|
||||
{
|
||||
return Vector3<T>( a[1] * b[2] - b[1] * a[2], -a[0] * b[2] + b[0] * a[2], a[0] * b[1] - b[0] * a[1] );
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline T dot( Vector3<T> const & a, Vector3<T> const & b ) { return a(0)*b(0) + a(1)*b(1) + a(2)*b(2); }
|
||||
|
||||
template <typename T>
|
||||
inline T inner_prod( Vector3<T> const & a, Vector3<T> const & b ) { return dot(a,b); }
|
||||
|
||||
template<typename T>
|
||||
inline T length(Vector3<T> const & v)
|
||||
{
|
||||
using std::sqrt;
|
||||
return (T)( sqrt( dot(v,v) ) );
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline T sqr_length(Vector3<T> const & v) { return (T)(v*v); }
|
||||
|
||||
template<typename T>
|
||||
inline T norm(Vector3<T> const & v) { return length(v); }
|
||||
|
||||
template<typename T>
|
||||
inline T norm_1(Vector3<T> const & v)
|
||||
{
|
||||
using std::fabs;
|
||||
using std::max;
|
||||
return max( fabs(v(0)), max( fabs(v(1)), fabs(v(2)) ) );
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline T distance(Vector3<T> const & a, Vector3<T> const & b)
|
||||
{
|
||||
return length(b-a);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline T sqr_distance(Vector3<T> const & a, Vector3<T> const & b)
|
||||
{
|
||||
return sqr_length(b-a);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline Vector3<T> sgn( Vector3<T> const & v )
|
||||
{
|
||||
using OpenTissue::math::sgn;
|
||||
return Vector3<T>(sgn(v(0)), sgn(v(1)), sgn(v(2)));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline Vector3<T> unit( Vector3<T> const & v )
|
||||
{
|
||||
typedef typename Vector3<T>::value_traits value_traits;
|
||||
|
||||
|
||||
using std::sqrt;
|
||||
T const l = length(v);
|
||||
if (l <= value_traits::zero())
|
||||
{
|
||||
return Vector3<T>( value_traits::zero() );
|
||||
}
|
||||
T const inv = value_traits::one()/l;
|
||||
return Vector3<T>( inv*v(0), inv*v(1), inv*v(2) );
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline Vector3<T> normalize( Vector3<T> const & v ) { return unit(v); }
|
||||
|
||||
template <typename T>
|
||||
inline void truncate(Vector3<T> & v, T const & precision_value)
|
||||
{
|
||||
typedef typename Vector3<T>::value_traits value_traits;
|
||||
|
||||
v(0) = ((v(0)>-precision_value)&&(v(0)<precision_value)) ? value_traits::zero() : v(0);
|
||||
v(1) = ((v(1)>-precision_value)&&(v(1)<precision_value)) ? value_traits::zero() : v(1);
|
||||
v(2) = ((v(2)>-precision_value)&&(v(2)<precision_value)) ? value_traits::zero() : v(2);
|
||||
}
|
||||
|
||||
|
||||
template <typename T>
|
||||
inline void truncate(Vector3<T> & v)
|
||||
{
|
||||
typedef typename Vector3<T>::value_traits value_traits;
|
||||
|
||||
truncate( v, value_traits::zero() );
|
||||
}
|
||||
|
||||
/**
|
||||
* Get Orthogonal Vector.
|
||||
*
|
||||
* @param v Any vector
|
||||
*
|
||||
* @return A vector orthogonal to v.
|
||||
*/
|
||||
template <typename T>
|
||||
inline Vector3<T> orthogonal(Vector3<T> const & v)
|
||||
{
|
||||
typedef typename Vector3<T>::value_traits value_traits;
|
||||
|
||||
using std::fabs;
|
||||
|
||||
//--- Find a vector not collinear with v, we simply pick the
|
||||
//--- y-axis direction as our prefered direction, afterwards
|
||||
//--- we test if this was a good choice if not we pick the
|
||||
//--- x-axis direction
|
||||
Vector3<T> tmp;
|
||||
if (fabs(v(1)) > fabs(v(0)))
|
||||
tmp = Vector3<T>(value_traits::one(),value_traits::zero(),value_traits::zero());
|
||||
else
|
||||
tmp = Vector3<T>(value_traits::zero(),value_traits::zero(),value_traits::one());
|
||||
//--- Now we find an orthogonal vector by using
|
||||
//--- the cross product
|
||||
return cross(v,tmp);
|
||||
}
|
||||
|
||||
template <typename T, typename T2>
|
||||
inline Vector3<T> operator*( Vector3<T> const& v, T2 const& s ) { return Vector3<T>( v(0)*s, v(1)*s, v(2)*s ); }
|
||||
|
||||
template <typename T2, typename T>
|
||||
inline Vector3<T> operator*( T2 const& s, Vector3<T> const& v ) { return Vector3<T>( v(0)*s, v(1)*s, v(2)*s ); }
|
||||
|
||||
template <typename T, typename T2>
|
||||
inline Vector3<T> operator/( Vector3<T> const& v, T2 const& s ) { return Vector3<T>( v(0)/s, v(1)/s, v(2)/s ); }
|
||||
|
||||
/**
|
||||
* Compute Orthonormal Vectors.
|
||||
* Compute unit vectors of a right handed coordinate frame, given initial z-axis (k-vector).
|
||||
*
|
||||
* @param i Upon return contains a orthogonal vector to j and k
|
||||
* @param j Upon return contains a orthogonal vector to i and k
|
||||
* @param k A given direction for the last vector, assumed to be a unit-vector.
|
||||
*/
|
||||
template<typename vector3_type>
|
||||
inline void orthonormal_vectors( vector3_type & i, vector3_type & j, vector3_type const & k )
|
||||
{
|
||||
typedef typename vector3_type::value_traits value_traits;
|
||||
|
||||
using std::fabs;
|
||||
vector3_type m_abs_k = fabs( k);
|
||||
|
||||
if ( m_abs_k( 0 ) > m_abs_k( 1 ) )
|
||||
{
|
||||
if ( m_abs_k( 0 ) > m_abs_k( 2 ) )
|
||||
i = vector3_type( value_traits::zero(), value_traits::one(), value_traits::zero() );
|
||||
else
|
||||
i = vector3_type( value_traits::one(), value_traits::zero(), value_traits::zero() );
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( m_abs_k( 1 ) > m_abs_k( 2 ) )
|
||||
i = vector3_type( value_traits::zero(), value_traits::zero(), value_traits::one() );
|
||||
else
|
||||
i = vector3_type( value_traits::one(), value_traits::zero(), value_traits::zero() );
|
||||
}
|
||||
j = unit( cross(k,i) );
|
||||
i = cross(j,k);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get increasing order of vector elements as a permutation array.
|
||||
*
|
||||
* Note this template function generalizes to any size of vectors
|
||||
* (not just 3-dimensional ones).
|
||||
*
|
||||
* @param v The vector to be examined.
|
||||
* @param pi Upon return this container contains the permuation
|
||||
* order, v(pi[0]) is smallest element of v, v(pi[1]) the
|
||||
* next smallest and so on.
|
||||
*/
|
||||
template<typename vector_type,typename permutation_container>
|
||||
inline void get_increasing_order( vector_type const & v, permutation_container & pi )
|
||||
{
|
||||
unsigned int n = v.size();
|
||||
|
||||
for(unsigned int i=0u;i<n;++i)
|
||||
pi[i] = i;
|
||||
|
||||
//--- Use insertion sort to find the increasing order of elements in the vector.
|
||||
for ( unsigned int i = 1u;i < n; ++i )
|
||||
{
|
||||
for ( unsigned int j = i;j > 0u;--j )
|
||||
{
|
||||
if ( v( pi[ j ] ) < v( pi[ j - 1 ] ) )
|
||||
{
|
||||
unsigned int tmp = pi[ j - 1 ];
|
||||
pi[ j - 1 ] = pi[ j ];
|
||||
pi[ j ] = tmp;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
inline Vector3<T> const & axis_x()
|
||||
{
|
||||
static Vector3<T> const xa(one<T>(), zero<T>(), zero<T>());
|
||||
return xa;
|
||||
}
|
||||
|
||||
|
||||
template <typename T>
|
||||
inline Vector3<T> const & axis_y()
|
||||
{
|
||||
static Vector3<T> const ya(zero<T>(), one<T>(), zero<T>());
|
||||
return ya;
|
||||
}
|
||||
|
||||
|
||||
template <typename T>
|
||||
inline Vector3<T> const & axis_z()
|
||||
{
|
||||
static Vector3<T> const za(zero<T>(), zero<T>(), one<T>());
|
||||
return za;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
} // namespace OpenTissue
|
||||
|
||||
//OPENTISSUE_CORE_MATH_MATH_VECTORX3_H
|
||||
#endif
|
||||
Reference in New Issue
Block a user