From e4f64ce098dbe30b9399da1a519b4c6de74f6911 Mon Sep 17 00:00:00 2001 From: Gael Guennebaud Date: Sat, 7 Mar 2009 13:52:44 +0000 Subject: [PATCH] add optimized quaternion * quaternion product specialization for float/SSE using code from Rohit Garg --- Eigen/Geometry | 4 ++ Eigen/src/Core/util/Constants.h | 12 ++++++ Eigen/src/Geometry/Quaternion.h | 21 +++++++---- Eigen/src/Geometry/arch/Geometry_SSE.h | 51 ++++++++++++++++++++++++++ 4 files changed, 81 insertions(+), 7 deletions(-) create mode 100644 Eigen/src/Geometry/arch/Geometry_SSE.h diff --git a/Eigen/Geometry b/Eigen/Geometry index 61aa70a91..e232ab683 100644 --- a/Eigen/Geometry +++ b/Eigen/Geometry @@ -45,6 +45,10 @@ namespace Eigen { #include "src/Geometry/ParametrizedLine.h" #include "src/Geometry/AlignedBox.h" +#if defined EIGEN_VECTORIZE_SSE + #include "src/Geometry/arch/Geometry_SSE.h" +#endif + } // namespace Eigen #include "src/Core/util/EnableMSVCWarnings.h" diff --git a/Eigen/src/Core/util/Constants.h b/Eigen/src/Core/util/Constants.h index 5d0aafda9..1dbba1bea 100644 --- a/Eigen/src/Core/util/Constants.h +++ b/Eigen/src/Core/util/Constants.h @@ -247,4 +247,16 @@ enum TransformTraits { Projective = 0x30 }; +const int EiArch_Generic = 0x0; +const int EiArch_SSE = 0x1; +const int EiArch_AltiVec = 0x2; + +#if defined EIGEN_VECTORIZE_SSE + const int EiArch = EiArch_SSE; +#elif defined EIGEN_VECTORIZE_ALTIVEC + const int EiArch = EiArch_AltiVec; +#else + const int EiArch = EiArch_Generic; +#endif + #endif // EIGEN_CONSTANTS_H diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index 0305fe176..b017bca75 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h @@ -225,17 +225,24 @@ typedef Quaternion Quaternionf; * double precision quaternion type */ typedef Quaternion Quaterniond; +// Generic Quaternion * Quaternion product +template inline Quaternion +ei_quaternion_product(const Quaternion& a, const Quaternion& b) +{ + return Quaternion + ( + a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(), + a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(), + a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(), + a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x() + ); +} + /** \returns the concatenation of two rotations as a quaternion-quaternion product */ template inline Quaternion Quaternion::operator* (const Quaternion& other) const { - return Quaternion - ( - this->w() * other.w() - this->x() * other.x() - this->y() * other.y() - this->z() * other.z(), - this->w() * other.x() + this->x() * other.w() + this->y() * other.z() - this->z() * other.y(), - this->w() * other.y() + this->y() * other.w() + this->z() * other.x() - this->x() * other.z(), - this->w() * other.z() + this->z() * other.w() + this->x() * other.y() - this->y() * other.x() - ); + return ei_quaternion_product(*this,other); } /** \sa operator*(Quaternion) */ diff --git a/Eigen/src/Geometry/arch/Geometry_SSE.h b/Eigen/src/Geometry/arch/Geometry_SSE.h new file mode 100644 index 000000000..6c2933898 --- /dev/null +++ b/Eigen/src/Geometry/arch/Geometry_SSE.h @@ -0,0 +1,51 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2009 Rohit Garg +// Copyright (C) 2009 Gael Guennebaud +// +// Eigen is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 3 of the License, or (at your option) any later version. +// +// Alternatively, you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of +// the License, or (at your option) any later version. +// +// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public +// License and a copy of the GNU General Public License along with +// Eigen. If not, see . + +#ifndef EIGEN_GEOMETRY_SSE_H +#define EIGEN_GEOMETRY_SSE_H + +#define vec4f_swizzle(v,p,q,r,s) (_mm_castsi128_ps(_mm_shuffle_epi32( _mm_castps_si128(v), \ + ((s)<<6|(r)<<4|(q)<<2|(p))))) + +template<> inline Quaternion +ei_quaternion_product(const Quaternion& _a, const Quaternion& _b) +{ + const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0,0,0,0x80000000)); + Quaternion res; + __m128 a = _a.coeffs().packet(0); + __m128 b = _b.coeffs().packet(0); + __m128 flip1 = _mm_xor_ps(_mm_mul_ps(vec4f_swizzle(a,1,2,0,2), + vec4f_swizzle(b,2,0,1,2)),mask); + __m128 flip2 = _mm_xor_ps(_mm_mul_ps(vec4f_swizzle(a,3,3,3,1), + vec4f_swizzle(b,0,1,2,1)),mask); + ei_pstore(&res.x(), + _mm_add_ps(_mm_sub_ps(_mm_mul_ps(a,vec4f_swizzle(b,3,3,3,3)), + _mm_mul_ps(vec4f_swizzle(a,2,0,1,0), + vec4f_swizzle(b,1,2,0,0))), + _mm_add_ps(flip1,flip2))); + return res; +} + +#endif // EIGEN_GEOMETRY_SSE_H