include/Vector3D.hpp

/******************************************************************************\
 * Copyright (c) 2011 Hans-Christian Ahlswede                                 *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 *                                                                            *
 *  §1 Redistributions of source code must retain the above copyright notice, *
 *     this list of conditions and the following disclaimer.                  *
 *                                                                            *
 *  §2 Redistributions in binary form must reproduce the above copyright      *
 *     notice, this list of conditions and the following disclaimer in the    *
 *     documentation and/or other materials provided with the distribution.   *
 *                                                                            *
 *  §3 The name of the author may not be used to endorse or promote products  *
 *     derived from this software without specific prior written permission.  *
 *                                                                            *
 *  §4 In the following paragraphs the term ``significant modification'' will *
 *     be used, which is defined as follows from now on:                      *
 *      (1) A significant modification is the modification or extension of    *
 *          functionality of the software by modifying or extending the       *
 *          source code.                                                      *
 *      (2) A significant modification is NOT the reduction of functionality  *
 *          of the software by modifying, extending or reducing the source    *
 *          code or the binary.                                               *
 *                                                                            *
 *  §5 In case of a ``significant modification'' of the software the modifier *
 *     is allowed to append a new line below the copyright line containing    *
 *     the period of modification, his name and other personal information.   *
 *                                                                            *
 *  §6 In case of a ``significant modification'' of the software this list    *
 *     of conditions can be extended to new parapgraphs by the modifier,      *
 *     located below the latest defined paragraph, if the new paragraphs      *
 *     do not affect or modify §1, §2, §3, §4, §5, §6, all paragraphs         *
 *     listed before the new paragraphs as well as the copyright notice       *
 *     and the disclaimer.                                                    *
 *                                                                            *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR       *
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES  *
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN *
 * NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,  *
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED   *
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR     *
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF     *
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING       *
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS         *
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.               *
\******************************************************************************/


__forceinline
Vector3D_t Vector3D ()
{
  register Vector3D_t V0;
#ifdef __SSE__ // SSE enabled
  // perform addition
  V0._ = _mm_setzero_ps();
#else // SSE disabled
  V0.v.x = 0.0f;
  V0.v.y = 0.0f;
  V0.v.z = 0.0f;
#endif
  return V0;
}


__forceinline
Vector3D_t Vector3D (register float S0)
{
  register Vector3D_t V0;
#ifdef __SSE__ // SSE enabled
  // perform addition
  V0._ = _mm_set_ps1 (S0);
#else // SSE disabled
  V0.v.x = S0;
  V0.v.y = S0;
  V0.v.z = S0;
#endif
  return V0;
}


__forceinline
Vector3D_t Vector3D (register float x, register float y, register float z)
{
  register Vector3D_t V0;
#ifdef __SSE__ // SSE enabled
  // perform addition
  V0._ = _mm_set_ps (x, y, z, 0.0f);
#else // SSE disabled
  V0.v.x = x;
  V0.v.y = y;
  V0.v.z = z;
#endif
  return V0;
}


__forceinline
Vector3D_t add (register Vector3D_t L0, register Vector3D_t R0)
{
#ifdef __SSE__ // SSE enabled
  // perform addition
  L0._ = _mm_add_ps (L0._, R0._);
#else // SSE disabled
  L0.v.x += R0.v.x;
  L0.v.y += R0.v.y;
  L0.v.z += R0.v.z;
#endif
  return L0;
}


__forceinline
void add_ (register Vector3D_t &L0, register Vector3D_t R0)
{
#ifdef __SSE__ // SSE enabled
  // perform addition
  L0._ = _mm_add_ps (L0._, R0._);
#else // SSE disabled
  L0.v.x += R0.v.x;
  L0.v.y += R0.v.y;
  L0.v.z += R0.v.z;
#endif
}


__forceinline
Vector3D_t sub (register Vector3D_t L0, register Vector3D_t R0)
{
#ifdef __SSE__ // SSE enabled
  // perform subtraction
  L0._ = _mm_sub_ps (L0._, R0._);
#else // SSE disabled
  L0.v.x -= R0.v.x;
  L0.v.y -= R0.v.y;
  L0.v.z -= R0.v.z;
#endif
  return L0;
}


__forceinline
void sub_ (register Vector3D_t &L0, register Vector3D_t R0)
{
#ifdef __SSE__ // SSE enabled
  // perform subtraction
  L0._ = _mm_sub_ps (L0._, R0._);
#else // SSE disabled
  L0.v.x -= R0.v.x;
  L0.v.y -= R0.v.y;
  L0.v.z -= R0.v.z;
#endif
}


__forceinline
Vector3D_t mul (register Vector3D_t L0, register float R0)
{
#ifdef __SSE__ // SSE enabled
  // associate registers
  register Vector3D_t R1;
  // prepare vector
  R1._ = _mm_set_ps1 (R0);
  // perform multiplication
  L0._  = _mm_mul_ps (L0._, R1._);
#else // SSE disabled
  L0.v.x *= R0;
  L0.v.y *= R0;
  L0.v.z *= R0;
#endif
  return L0;
}


__forceinline
void mul_ (register Vector3D_t &L0, register float R0)
{
#ifdef __SSE__ // SSE enabled
  // associate registers
  register Vector3D_t R1;
  // prepare vector
  R1._ = _mm_set_ps1 (R0);
  // perform multiplication
  L0._  = _mm_mul_ps (L0._, R1._);
#else // SSE disabled
  L0.v.x *= R0;
  L0.v.y *= R0;
  L0.v.z *= R0;
#endif
}


__forceinline
Vector3D_t div (register Vector3D_t L0, register float R0)
{
#ifdef __SSE__ // SSE enabled
  // associate registers
  register Vector3D_t R1;
  // prepare vector
  R1._ = _mm_set_ps1 (R0);
  // perform multiplication
  L0._  = _mm_div_ps (L0._, R1._);
#else // SSE disabled
  L0.v.x /= R0;
  L0.v.y /= R0;
  L0.v.z /= R0;
#endif
  return L0;
}


__forceinline
void div_ (register Vector3D_t &L0, register float R0)
{
#ifdef __SSE__ // SSE enabled
  // associate registers
  register Vector3D_t R1;
  // prepare vector
  R1._ = _mm_set_ps1 (R0);
  // perform multiplication
  L0._  = _mm_div_ps (L0._, R1._);
#else // SSE disabled
  L0.v.x /= R0;
  L0.v.y /= R0;
  L0.v.z /= R0;
#endif
}


__forceinline
float dot (register Vector3D_t L0, register Vector3D_t R0)
{
  // associate registers I
  register float S0;
#ifdef __SSE__ // SSE enabled
#if defined(__SSE4_1__) || defined(__SSE4_2__) // calculate dot product with SSE4
  L0._ = _mm_dp_ps (L0._, R0._, VECTOR3D_DOTP_MASK);
#else // calculate dot product without SSE4
  // multiply vector elements
  L0._ = _mm_mul_ps (L0._, R0._);
#ifdef __SSE3__ // SSE3 enabled
  // multiply vector elements horizontal
  // (Yes, performing "_mm_hadd_ps twice()" is correct!)
  L0._ = _mm_hadd_ps (L0._, L0._);
  L0._ = _mm_hadd_ps (L0._, L0._);
#else // SSE3 disabled
  // associate registers II
  register Vector3D_t V0;
  // shuffle vectors
  R0._ = _mm_shuffle_ps (L0._, R0._, VECTOR3D_ROT1_MASK);
  V0._ = _mm_shuffle_ps (L0._, V0._, VECTOR3D_ROT2_MASK);
  // sum shuffled vectors
  V0._ = _mm_add_ss (V0._, R0._);
  L0._ = _mm_add_ss (L0._, V0._);
#endif
#endif
  // get the result
  _mm_store_ss (&S0, L0._);
#else // SSE disabled
  S0  = L0.v.x * R0.v.x;
  S0 += L0.v.y * R0.v.y;
  S0 += L0.v.z * R0.v.z;
#endif
  // return resulting value
  return S0;
}


__forceinline
Vector3D_t cross (register Vector3D_t L0, register Vector3D_t R0)
{
#ifdef __SSE__ // SSE enabled
  // associate registers
  register Vector3D_t L1;
  register Vector3D_t R1;
  L1._ = L0._;
  R1._ = R0._;
  // shuffle elements
  L0._ = _mm_shuffle_ps (L0._, L0._, VECTOR3D_ROT1_MASK);
  R1._ = _mm_shuffle_ps (R1._, R1._, VECTOR3D_ROT1_MASK);
  R0._ = _mm_shuffle_ps (R0._, R0._, VECTOR3D_ROT2_MASK);
  L1._ = _mm_shuffle_ps (L1._, L1._, VECTOR3D_ROT2_MASK);
  // multiply vectors
  L0._ = _mm_mul_ps (L0._, R0._ );
  L1._ = _mm_mul_ps (L1._, R1._);
  // subtract resulting vectors
  L0._ = _mm_sub_ps (L0._, L1._);
  // return new vector
  return L0;
#else // SSE disabled
  // create empty vector
  register Vector3D_t V0;
  // calculate cross product
  V0.v.x  = L0.v.y * R0.v.z;
  V0.v.y  = L0.v.z * R0.v.x;
  V0.v.z  = L0.v.x * R0.v.y;
  V0.v.x -= R0.v.y * L0.v.z;
  V0.v.y -= R0.v.z * L0.v.x;
  V0.v.z -= R0.v.x * L0.v.y;
//   V0.v._  = 0.0f;
  return V0;
#endif
}


__forceinline
void cross_ (register Vector3D_t &L0, register Vector3D_t R0)
{
#ifdef __SSE__ // SSE enabled
  // associate registers
  register Vector3D_t L1;
  register Vector3D_t R1;
  L1._ = L0._;
  R1._ = R0._;
  // shuffle elements
  L0._ = _mm_shuffle_ps (L0._, L0._, VECTOR3D_ROT1_MASK);
  R1._ = _mm_shuffle_ps (R1._, R1._, VECTOR3D_ROT1_MASK);
  R0._ = _mm_shuffle_ps (R0._, R0._, VECTOR3D_ROT2_MASK);
  L1._ = _mm_shuffle_ps (L1._, L1._, VECTOR3D_ROT2_MASK);
  // multiply vectors
  L0._ = _mm_mul_ps (L0._, R0._ );
  L1._ = _mm_mul_ps (L1._, R1._);
  // subtract resulting vectors
  L0._ = _mm_sub_ps (L0._, L1._);
#else // SSE disabled
  // create empty vector
  register Vector3D_t V0;
  // calculate cross product
  V0.v.x  = L0.v.y * R0.v.z;
  V0.v.y  = L0.v.z * R0.v.x;
  V0.v.z  = L0.v.x * R0.v.y;
  V0.v.x -= R0.v.y * L0.v.z;
  V0.v.y -= R0.v.z * L0.v.x;
  V0.v.z -= R0.v.x * L0.v.y;
  L0.v.x  = V0.v.x;
  L0.v.y  = V0.v.y;
  L0.v.z  = V0.v.z;
//   L0.v._  = 0.0f;
#endif
}


__forceinline
float triple (register Vector3D_t L0, register Vector3D_t M0, register Vector3D_t R0)
{
  register float S0;
#ifdef __SSE__ // SSE enabled
  // associate registers
  register Vector3D_t M1;
  register Vector3D_t R1;
  M1._ = M0._;
  R1._ = R0._;
  // shuffle elements
  M0._ = _mm_shuffle_ps (M0._, M0._ , VECTOR3D_ROT1_MASK);
  R1._ = _mm_shuffle_ps (R1._, R1._, VECTOR3D_ROT1_MASK);
  R0._ = _mm_shuffle_ps (R0._, R0._ , VECTOR3D_ROT2_MASK);
  M1._ = _mm_shuffle_ps (M1._, M1._, VECTOR3D_ROT2_MASK);
  // multiply vectors
  M0._ = _mm_mul_ps (M0._, R0._ );
  M1._ = _mm_mul_ps (M1._, R1._);
  // subtract resulting vectors
  M0._ = _mm_sub_ps (M0._, M1._);
#if defined(__SSE4_1__) || defined(__SSE4_2__) // calculate dot product with SSE4
  L0._ = _mm_dp_ps (L0._, M0._, VECTOR3D_DOTP_MASK);
#else // calculate dot product without SSE4
  // multiply vector elements
  L0._ = _mm_mul_ps (L0._, M0._);
#ifdef __SSE3__ // SSE3 enabled
  // multiply vector elements horizontal
  // (Yes, performing "_mm_hadd_ps twice()" is correct!)
  L0._ = _mm_hadd_ps (L0._, L0._);
  L0._ = _mm_hadd_ps (L0._, L0._);
#else // SSE3 disabled
  // shuffle vectors
  M0._ = _mm_shuffle_ps (L0._, M0._, VECTOR3D_ROT1_MASK);
  R0._ = _mm_shuffle_ps (L0._, R0._, VECTOR3D_ROT2_MASK);
  // sum shuffled vectors
  R0._ = _mm_add_ss (R0._, M0._);
  L0._ = _mm_add_ss (L0._, R0._);
#endif
#endif
  // get the result
  _mm_store_ss (&S0, L0._);
#else // SSE disabled
  // create empty vector
  register Vector3D_t V0;
  // calculate cross product
  V0.v.x  = M0.v.y * R0.v.z;
  V0.v.y  = M0.v.z * R0.v.x;
  V0.v.z  = M0.v.x * R0.v.y;
  V0.v.x -= R0.v.y * M0.v.z;
  V0.v.y -= R0.v.z * M0.v.x;
  V0.v.z -= R0.v.x * M0.v.y;
  // calculate dot product
  S0  = L0.v.x * V0.v.x;
  S0 += L0.v.y * V0.v.y;
  S0 += L0.v.z * V0.v.z;
#endif
  return S0;
}


__forceinline
float abs (register Vector3D_t V0)
{
  // associate registers I
  register float S0;
#ifdef __SSE__ // SSE enabled
#if defined(__SSE4_1__) || defined(__SSE4_2__) // calculate dot product with SSE4
  V0._ = _mm_dp_ps (V0._, V0._, VECTOR3D_DOTP_MASK);
#else // calculate dot product without SSE4
  // multiply vector elements
  V0._ = _mm_mul_ps (V0._, V0._);
#ifdef __SSE3__ // SSE3 enabled
  // multiply vector elements horizontal
  // (Yes, performing "_mm_hadd_ps twice()" is correct!)
  V0._ = _mm_hadd_ps (V0._, V0._);
  V0._ = _mm_hadd_ps (V0._, V0._);
#else // SSE3 disabled
  // associate registers II
  register Vector3D_t V1;
  register Vector3D_t V2;
  // shuffle vectors
  V1._ = _mm_shuffle_ps (V0._, V1._, VECTOR3D_ROT1_MASK);
  V2._ = _mm_shuffle_ps (V0._, V2._, VECTOR3D_ROT2_MASK);
  // sum shuffled vectors
  V2._ = _mm_add_ss (V2._, V1._);
  V0._ = _mm_add_ss (V0._, V2._);
#endif
#endif
  // calculate square root
  V0._ = _mm_sqrt_ss (V0._);
  // get the result
  _mm_store_ss (&S0, V0._);
#else // SSE disabled
  S0  = V0.v.x * V0.v.x;
  S0 += V0.v.y * V0.v.y;
  S0 += V0.v.z * V0.v.z;
  S0  = sqrtf (S0);
#endif
  // return resulting value
  return S0;
}


__forceinline
Vector3D_t unit (register Vector3D_t V0)
{
  return div ( V0, abs(V0) );
}


__forceinline
void unit_ (register Vector3D_t &V0)
{
  div_ ( V0, abs(V0) );
}