MathFunctions.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2007 Julien Pommier
00005 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
00006 //
00007 // Eigen is free software; you can redistribute it and/or
00008 // modify it under the terms of the GNU Lesser General Public
00009 // License as published by the Free Software Foundation; either
00010 // version 3 of the License, or (at your option) any later version.
00011 //
00012 // Alternatively, you can redistribute it and/or
00013 // modify it under the terms of the GNU General Public License as
00014 // published by the Free Software Foundation; either version 2 of
00015 // the License, or (at your option) any later version.
00016 //
00017 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00018 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00019 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00020 // GNU General Public License for more details.
00021 //
00022 // You should have received a copy of the GNU Lesser General Public
00023 // License and a copy of the GNU General Public License along with
00024 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00025 
00026 /* The sin, cos, exp, and log functions of this file come from
00027  * Julien Pommier's sse math library: http://gruntthepeon.free.fr/ssemath/
00028  */
00029 
00030 #ifndef EIGEN_MATH_FUNCTIONS_SSE_H
00031 #define EIGEN_MATH_FUNCTIONS_SSE_H
00032 
00033 namespace Eigen {
00034 
00035 namespace internal {
00036 
00037 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
00038 Packet4f plog<Packet4f>(const Packet4f& _x)
00039 {
00040   Packet4f x = _x;
00041   _EIGEN_DECLARE_CONST_Packet4f(1 , 1.0f);
00042   _EIGEN_DECLARE_CONST_Packet4f(half, 0.5f);
00043   _EIGEN_DECLARE_CONST_Packet4i(0x7f, 0x7f);
00044 
00045   _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(inv_mant_mask, ~0x7f800000);
00046 
00047   /* the smallest non denormalized float number */
00048   _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(min_norm_pos,  0x00800000);
00049 
00050   /* natural logarithm computed for 4 simultaneous float
00051     return NaN for x <= 0
00052   */
00053   _EIGEN_DECLARE_CONST_Packet4f(cephes_SQRTHF, 0.707106781186547524f);
00054   _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p0, 7.0376836292E-2f);
00055   _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p1, - 1.1514610310E-1f);
00056   _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p2, 1.1676998740E-1f);
00057   _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p3, - 1.2420140846E-1f);
00058   _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p4, + 1.4249322787E-1f);
00059   _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p5, - 1.6668057665E-1f);
00060   _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p6, + 2.0000714765E-1f);
00061   _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p7, - 2.4999993993E-1f);
00062   _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p8, + 3.3333331174E-1f);
00063   _EIGEN_DECLARE_CONST_Packet4f(cephes_log_q1, -2.12194440e-4f);
00064   _EIGEN_DECLARE_CONST_Packet4f(cephes_log_q2, 0.693359375f);
00065 
00066 
00067   Packet4i emm0;
00068 
00069   Packet4f invalid_mask = _mm_cmple_ps(x, _mm_setzero_ps());
00070 
00071   x = pmax(x, p4f_min_norm_pos);  /* cut off denormalized stuff */
00072   emm0 = _mm_srli_epi32(_mm_castps_si128(x), 23);
00073 
00074   /* keep only the fractional part */
00075   x = _mm_and_ps(x, p4f_inv_mant_mask);
00076   x = _mm_or_ps(x, p4f_half);
00077 
00078   emm0 = _mm_sub_epi32(emm0, p4i_0x7f);
00079   Packet4f e = padd(_mm_cvtepi32_ps(emm0), p4f_1);
00080 
00081   /* part2:
00082      if( x < SQRTHF ) {
00083        e -= 1;
00084        x = x + x - 1.0;
00085      } else { x = x - 1.0; }
00086   */
00087   Packet4f mask = _mm_cmplt_ps(x, p4f_cephes_SQRTHF);
00088   Packet4f tmp = _mm_and_ps(x, mask);
00089   x = psub(x, p4f_1);
00090   e = psub(e, _mm_and_ps(p4f_1, mask));
00091   x = padd(x, tmp);
00092 
00093   Packet4f x2 = pmul(x,x);
00094   Packet4f x3 = pmul(x2,x);
00095 
00096   Packet4f y, y1, y2;
00097   y  = pmadd(p4f_cephes_log_p0, x, p4f_cephes_log_p1);
00098   y1 = pmadd(p4f_cephes_log_p3, x, p4f_cephes_log_p4);
00099   y2 = pmadd(p4f_cephes_log_p6, x, p4f_cephes_log_p7);
00100   y  = pmadd(y , x, p4f_cephes_log_p2);
00101   y1 = pmadd(y1, x, p4f_cephes_log_p5);
00102   y2 = pmadd(y2, x, p4f_cephes_log_p8);
00103   y = pmadd(y, x3, y1);
00104   y = pmadd(y, x3, y2);
00105   y = pmul(y, x3);
00106 
00107   y1 = pmul(e, p4f_cephes_log_q1);
00108   tmp = pmul(x2, p4f_half);
00109   y = padd(y, y1);
00110   x = psub(x, tmp);
00111   y2 = pmul(e, p4f_cephes_log_q2);
00112   x = padd(x, y);
00113   x = padd(x, y2);
00114   return _mm_or_ps(x, invalid_mask); // negative arg will be NAN
00115 }
00116 
00117 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
00118 Packet4f pexp<Packet4f>(const Packet4f& _x)
00119 {
00120   Packet4f x = _x;
00121   _EIGEN_DECLARE_CONST_Packet4f(1 , 1.0f);
00122   _EIGEN_DECLARE_CONST_Packet4f(half, 0.5f);
00123   _EIGEN_DECLARE_CONST_Packet4i(0x7f, 0x7f);
00124 
00125 
00126   _EIGEN_DECLARE_CONST_Packet4f(exp_hi, 88.3762626647949f);
00127   _EIGEN_DECLARE_CONST_Packet4f(exp_lo, -88.3762626647949f);
00128 
00129   _EIGEN_DECLARE_CONST_Packet4f(cephes_LOG2EF, 1.44269504088896341f);
00130   _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_C1, 0.693359375f);
00131   _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_C2, -2.12194440e-4f);
00132 
00133   _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p0, 1.9875691500E-4f);
00134   _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p1, 1.3981999507E-3f);
00135   _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p2, 8.3334519073E-3f);
00136   _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p3, 4.1665795894E-2f);
00137   _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p4, 1.6666665459E-1f);
00138   _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p5, 5.0000001201E-1f);
00139 
00140   Packet4f tmp = _mm_setzero_ps(), fx;
00141   Packet4i emm0;
00142 
00143   // clamp x
00144   x = pmax(pmin(x, p4f_exp_hi), p4f_exp_lo);
00145 
00146   /* express exp(x) as exp(g + n*log(2)) */
00147   fx = pmadd(x, p4f_cephes_LOG2EF, p4f_half);
00148 
00149   /* how to perform a floorf with SSE: just below */
00150   emm0 = _mm_cvttps_epi32(fx);
00151   tmp  = _mm_cvtepi32_ps(emm0);
00152   /* if greater, substract 1 */
00153   Packet4f mask = _mm_cmpgt_ps(tmp, fx);
00154   mask = _mm_and_ps(mask, p4f_1);
00155   fx = psub(tmp, mask);
00156 
00157   tmp = pmul(fx, p4f_cephes_exp_C1);
00158   Packet4f z = pmul(fx, p4f_cephes_exp_C2);
00159   x = psub(x, tmp);
00160   x = psub(x, z);
00161 
00162   z = pmul(x,x);
00163 
00164   Packet4f y = p4f_cephes_exp_p0;
00165   y = pmadd(y, x, p4f_cephes_exp_p1);
00166   y = pmadd(y, x, p4f_cephes_exp_p2);
00167   y = pmadd(y, x, p4f_cephes_exp_p3);
00168   y = pmadd(y, x, p4f_cephes_exp_p4);
00169   y = pmadd(y, x, p4f_cephes_exp_p5);
00170   y = pmadd(y, z, x);
00171   y = padd(y, p4f_1);
00172 
00173   /* build 2^n */
00174   emm0 = _mm_cvttps_epi32(fx);
00175   emm0 = _mm_add_epi32(emm0, p4i_0x7f);
00176   emm0 = _mm_slli_epi32(emm0, 23);
00177   return pmul(y, _mm_castsi128_ps(emm0));
00178 }
00179 
00180 /* evaluation of 4 sines at onces, using SSE2 intrinsics.
00181 
00182    The code is the exact rewriting of the cephes sinf function.
00183    Precision is excellent as long as x < 8192 (I did not bother to
00184    take into account the special handling they have for greater values
00185    -- it does not return garbage for arguments over 8192, though, but
00186    the extra precision is missing).
00187 
00188    Note that it is such that sinf((float)M_PI) = 8.74e-8, which is the
00189    surprising but correct result.
00190 */
00191 
00192 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
00193 Packet4f psin<Packet4f>(const Packet4f& _x)
00194 {
00195   Packet4f x = _x;
00196   _EIGEN_DECLARE_CONST_Packet4f(1 , 1.0f);
00197   _EIGEN_DECLARE_CONST_Packet4f(half, 0.5f);
00198 
00199   _EIGEN_DECLARE_CONST_Packet4i(1, 1);
00200   _EIGEN_DECLARE_CONST_Packet4i(not1, ~1);
00201   _EIGEN_DECLARE_CONST_Packet4i(2, 2);
00202   _EIGEN_DECLARE_CONST_Packet4i(4, 4);
00203 
00204   _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(sign_mask, 0x80000000);
00205 
00206   _EIGEN_DECLARE_CONST_Packet4f(minus_cephes_DP1,-0.78515625f);
00207   _EIGEN_DECLARE_CONST_Packet4f(minus_cephes_DP2, -2.4187564849853515625e-4f);
00208   _EIGEN_DECLARE_CONST_Packet4f(minus_cephes_DP3, -3.77489497744594108e-8f);
00209   _EIGEN_DECLARE_CONST_Packet4f(sincof_p0, -1.9515295891E-4f);
00210   _EIGEN_DECLARE_CONST_Packet4f(sincof_p1,  8.3321608736E-3f);
00211   _EIGEN_DECLARE_CONST_Packet4f(sincof_p2, -1.6666654611E-1f);
00212   _EIGEN_DECLARE_CONST_Packet4f(coscof_p0,  2.443315711809948E-005f);
00213   _EIGEN_DECLARE_CONST_Packet4f(coscof_p1, -1.388731625493765E-003f);
00214   _EIGEN_DECLARE_CONST_Packet4f(coscof_p2,  4.166664568298827E-002f);
00215   _EIGEN_DECLARE_CONST_Packet4f(cephes_FOPI, 1.27323954473516f); // 4 / M_PI
00216 
00217   Packet4f xmm1, xmm2 = _mm_setzero_ps(), xmm3, sign_bit, y;
00218 
00219   Packet4i emm0, emm2;
00220   sign_bit = x;
00221   /* take the absolute value */
00222   x = pabs(x);
00223 
00224   /* take the modulo */
00225 
00226   /* extract the sign bit (upper one) */
00227   sign_bit = _mm_and_ps(sign_bit, p4f_sign_mask);
00228 
00229   /* scale by 4/Pi */
00230   y = pmul(x, p4f_cephes_FOPI);
00231 
00232   /* store the integer part of y in mm0 */
00233   emm2 = _mm_cvttps_epi32(y);
00234   /* j=(j+1) & (~1) (see the cephes sources) */
00235   emm2 = _mm_add_epi32(emm2, p4i_1);
00236   emm2 = _mm_and_si128(emm2, p4i_not1);
00237   y = _mm_cvtepi32_ps(emm2);
00238   /* get the swap sign flag */
00239   emm0 = _mm_and_si128(emm2, p4i_4);
00240   emm0 = _mm_slli_epi32(emm0, 29);
00241   /* get the polynom selection mask
00242      there is one polynom for 0 <= x <= Pi/4
00243      and another one for Pi/4<x<=Pi/2
00244 
00245      Both branches will be computed.
00246   */
00247   emm2 = _mm_and_si128(emm2, p4i_2);
00248   emm2 = _mm_cmpeq_epi32(emm2, _mm_setzero_si128());
00249 
00250   Packet4f swap_sign_bit = _mm_castsi128_ps(emm0);
00251   Packet4f poly_mask = _mm_castsi128_ps(emm2);
00252   sign_bit = _mm_xor_ps(sign_bit, swap_sign_bit);
00253 
00254   /* The magic pass: "Extended precision modular arithmetic"
00255      x = ((x - y * DP1) - y * DP2) - y * DP3; */
00256   xmm1 = pmul(y, p4f_minus_cephes_DP1);
00257   xmm2 = pmul(y, p4f_minus_cephes_DP2);
00258   xmm3 = pmul(y, p4f_minus_cephes_DP3);
00259   x = padd(x, xmm1);
00260   x = padd(x, xmm2);
00261   x = padd(x, xmm3);
00262 
00263   /* Evaluate the first polynom  (0 <= x <= Pi/4) */
00264   y = p4f_coscof_p0;
00265   Packet4f z = _mm_mul_ps(x,x);
00266 
00267   y = pmadd(y, z, p4f_coscof_p1);
00268   y = pmadd(y, z, p4f_coscof_p2);
00269   y = pmul(y, z);
00270   y = pmul(y, z);
00271   Packet4f tmp = pmul(z, p4f_half);
00272   y = psub(y, tmp);
00273   y = padd(y, p4f_1);
00274 
00275   /* Evaluate the second polynom  (Pi/4 <= x <= 0) */
00276 
00277   Packet4f y2 = p4f_sincof_p0;
00278   y2 = pmadd(y2, z, p4f_sincof_p1);
00279   y2 = pmadd(y2, z, p4f_sincof_p2);
00280   y2 = pmul(y2, z);
00281   y2 = pmul(y2, x);
00282   y2 = padd(y2, x);
00283 
00284   /* select the correct result from the two polynoms */
00285   y2 = _mm_and_ps(poly_mask, y2);
00286   y = _mm_andnot_ps(poly_mask, y);
00287   y = _mm_or_ps(y,y2);
00288   /* update the sign */
00289   return _mm_xor_ps(y, sign_bit);
00290 }
00291 
00292 /* almost the same as psin */
00293 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
00294 Packet4f pcos<Packet4f>(const Packet4f& _x)
00295 {
00296   Packet4f x = _x;
00297   _EIGEN_DECLARE_CONST_Packet4f(1 , 1.0f);
00298   _EIGEN_DECLARE_CONST_Packet4f(half, 0.5f);
00299 
00300   _EIGEN_DECLARE_CONST_Packet4i(1, 1);
00301   _EIGEN_DECLARE_CONST_Packet4i(not1, ~1);
00302   _EIGEN_DECLARE_CONST_Packet4i(2, 2);
00303   _EIGEN_DECLARE_CONST_Packet4i(4, 4);
00304 
00305   _EIGEN_DECLARE_CONST_Packet4f(minus_cephes_DP1,-0.78515625f);
00306   _EIGEN_DECLARE_CONST_Packet4f(minus_cephes_DP2, -2.4187564849853515625e-4f);
00307   _EIGEN_DECLARE_CONST_Packet4f(minus_cephes_DP3, -3.77489497744594108e-8f);
00308   _EIGEN_DECLARE_CONST_Packet4f(sincof_p0, -1.9515295891E-4f);
00309   _EIGEN_DECLARE_CONST_Packet4f(sincof_p1,  8.3321608736E-3f);
00310   _EIGEN_DECLARE_CONST_Packet4f(sincof_p2, -1.6666654611E-1f);
00311   _EIGEN_DECLARE_CONST_Packet4f(coscof_p0,  2.443315711809948E-005f);
00312   _EIGEN_DECLARE_CONST_Packet4f(coscof_p1, -1.388731625493765E-003f);
00313   _EIGEN_DECLARE_CONST_Packet4f(coscof_p2,  4.166664568298827E-002f);
00314   _EIGEN_DECLARE_CONST_Packet4f(cephes_FOPI, 1.27323954473516f); // 4 / M_PI
00315 
00316   Packet4f xmm1, xmm2 = _mm_setzero_ps(), xmm3, y;
00317   Packet4i emm0, emm2;
00318 
00319   x = pabs(x);
00320 
00321   /* scale by 4/Pi */
00322   y = pmul(x, p4f_cephes_FOPI);
00323 
00324   /* get the integer part of y */
00325   emm2 = _mm_cvttps_epi32(y);
00326   /* j=(j+1) & (~1) (see the cephes sources) */
00327   emm2 = _mm_add_epi32(emm2, p4i_1);
00328   emm2 = _mm_and_si128(emm2, p4i_not1);
00329   y = _mm_cvtepi32_ps(emm2);
00330 
00331   emm2 = _mm_sub_epi32(emm2, p4i_2);
00332 
00333   /* get the swap sign flag */
00334   emm0 = _mm_andnot_si128(emm2, p4i_4);
00335   emm0 = _mm_slli_epi32(emm0, 29);
00336   /* get the polynom selection mask */
00337   emm2 = _mm_and_si128(emm2, p4i_2);
00338   emm2 = _mm_cmpeq_epi32(emm2, _mm_setzero_si128());
00339 
00340   Packet4f sign_bit = _mm_castsi128_ps(emm0);
00341   Packet4f poly_mask = _mm_castsi128_ps(emm2);
00342 
00343   /* The magic pass: "Extended precision modular arithmetic"
00344      x = ((x - y * DP1) - y * DP2) - y * DP3; */
00345   xmm1 = pmul(y, p4f_minus_cephes_DP1);
00346   xmm2 = pmul(y, p4f_minus_cephes_DP2);
00347   xmm3 = pmul(y, p4f_minus_cephes_DP3);
00348   x = padd(x, xmm1);
00349   x = padd(x, xmm2);
00350   x = padd(x, xmm3);
00351 
00352   /* Evaluate the first polynom  (0 <= x <= Pi/4) */
00353   y = p4f_coscof_p0;
00354   Packet4f z = pmul(x,x);
00355 
00356   y = pmadd(y,z,p4f_coscof_p1);
00357   y = pmadd(y,z,p4f_coscof_p2);
00358   y = pmul(y, z);
00359   y = pmul(y, z);
00360   Packet4f tmp = _mm_mul_ps(z, p4f_half);
00361   y = psub(y, tmp);
00362   y = padd(y, p4f_1);
00363 
00364   /* Evaluate the second polynom  (Pi/4 <= x <= 0) */
00365   Packet4f y2 = p4f_sincof_p0;
00366   y2 = pmadd(y2, z, p4f_sincof_p1);
00367   y2 = pmadd(y2, z, p4f_sincof_p2);
00368   y2 = pmul(y2, z);
00369   y2 = pmadd(y2, x, x);
00370 
00371   /* select the correct result from the two polynoms */
00372   y2 = _mm_and_ps(poly_mask, y2);
00373   y  = _mm_andnot_ps(poly_mask, y);
00374   y  = _mm_or_ps(y,y2);
00375 
00376   /* update the sign */
00377   return _mm_xor_ps(y, sign_bit);
00378 }
00379 
00380 // This is based on Quake3's fast inverse square root.
00381 // For detail see here: http://www.beyond3d.com/content/articles/8/
00382 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
00383 Packet4f psqrt<Packet4f>(const Packet4f& _x)
00384 {
00385   Packet4f half = pmul(_x, pset1<Packet4f>(.5f));
00386 
00387   /* select only the inverse sqrt of non-zero inputs */
00388   Packet4f non_zero_mask = _mm_cmpgt_ps(_x, pset1<Packet4f>(std::numeric_limits<float>::epsilon()));
00389   Packet4f x = _mm_and_ps(non_zero_mask, _mm_rsqrt_ps(_x));
00390 
00391   x = pmul(x, psub(pset1<Packet4f>(1.5f), pmul(half, pmul(x,x))));
00392   return pmul(_x,x);
00393 }
00394 
00395 } // end namespace internal
00396 
00397 } // end namespace Eigen
00398 
00399 #endif // EIGEN_MATH_FUNCTIONS_SSE_H