geomancy-0.4.0.0: cbits/dmat4.c

#if defined(__SSE2__)

#include <emmintrin.h>

void DMat4xDMat4_SIMD(double *A, double *B, double *O) {
  __m128d row0lo = _mm_loadu_pd(&A[0]);
  __m128d row0hi = _mm_loadu_pd(&A[2]);
  __m128d row1lo = _mm_loadu_pd(&A[4]);
  __m128d row1hi = _mm_loadu_pd(&A[6]);
  __m128d row2lo = _mm_loadu_pd(&A[8]);
  __m128d row2hi = _mm_loadu_pd(&A[10]);
  __m128d row3lo = _mm_loadu_pd(&A[12]);
  __m128d row3hi = _mm_loadu_pd(&A[14]);
  for(int i=0; i<4; i++) {
    __m128d brod0 = _mm_set1_pd(B[4*i + 0]);
    __m128d brod1 = _mm_set1_pd(B[4*i + 1]);
    __m128d brod2 = _mm_set1_pd(B[4*i + 2]);
    __m128d brod3 = _mm_set1_pd(B[4*i + 3]);
    __m128d rowlo = _mm_add_pd(
      _mm_add_pd(
        _mm_mul_pd(brod0, row0lo),
        _mm_mul_pd(brod1, row1lo)),
      _mm_add_pd(
        _mm_mul_pd(brod2, row2lo),
        _mm_mul_pd(brod3, row3lo)));
    __m128d rowhi = _mm_add_pd(
      _mm_add_pd(
        _mm_mul_pd(brod0, row0hi),
        _mm_mul_pd(brod1, row1hi)),
      _mm_add_pd(
        _mm_mul_pd(brod2, row2hi),
        _mm_mul_pd(brod3, row3hi)));
    _mm_storeu_pd(&O[4*i], rowlo);
    _mm_storeu_pd(&O[4*i + 2], rowhi);
  }
}

void DMat4xDVec4_SIMD(double *M, double *V, double *O) {
  __m128d row0lo = _mm_loadu_pd(&M[0]);
  __m128d row0hi = _mm_loadu_pd(&M[2]);
  __m128d row1lo = _mm_loadu_pd(&M[4]);
  __m128d row1hi = _mm_loadu_pd(&M[6]);
  __m128d row2lo = _mm_loadu_pd(&M[8]);
  __m128d row2hi = _mm_loadu_pd(&M[10]);
  __m128d row3lo = _mm_loadu_pd(&M[12]);
  __m128d row3hi = _mm_loadu_pd(&M[14]);

  __m128d vec00 = _mm_set1_pd(V[0]);
  __m128d vec01 = _mm_set1_pd(V[1]);
  __m128d vec02 = _mm_set1_pd(V[2]);
  __m128d vec03 = _mm_set1_pd(V[3]);

  __m128d reslo = _mm_mul_pd(row0lo, vec00);
  reslo = _mm_add_pd(reslo, _mm_mul_pd(row1lo, vec01));
  reslo = _mm_add_pd(reslo, _mm_mul_pd(row2lo, vec02));
  reslo = _mm_add_pd(reslo, _mm_mul_pd(row3lo, vec03));

  __m128d reshi = _mm_mul_pd(row0hi, vec00);
  reshi = _mm_add_pd(reshi, _mm_mul_pd(row1hi, vec01));
  reshi = _mm_add_pd(reshi, _mm_mul_pd(row2hi, vec02));
  reshi = _mm_add_pd(reshi, _mm_mul_pd(row3hi, vec03));

  _mm_storeu_pd(&O[0], reslo);
  _mm_storeu_pd(&O[2], reshi);
}

#elif defined(__aarch64__)

#include <arm_neon.h>

void DMat4xDMat4_SIMD(double *A, double *B, double *O) {
  float64x2_t A0lo = vld1q_f64(&A[0]);
  float64x2_t A0hi = vld1q_f64(&A[2]);
  float64x2_t A1lo = vld1q_f64(&A[4]);
  float64x2_t A1hi = vld1q_f64(&A[6]);
  float64x2_t A2lo = vld1q_f64(&A[8]);
  float64x2_t A2hi = vld1q_f64(&A[10]);
  float64x2_t A3lo = vld1q_f64(&A[12]);
  float64x2_t A3hi = vld1q_f64(&A[14]);

  for(int i=0; i<4; i++) {
    float64x2_t Blo = vld1q_f64(&B[4*i]);
    float64x2_t Bhi = vld1q_f64(&B[4*i + 2]);

    float64x2_t Clo = vmovq_n_f64(0);
    Clo = vfmaq_laneq_f64(Clo, A0lo, Blo, 0);
    Clo = vfmaq_laneq_f64(Clo, A1lo, Blo, 1);
    Clo = vfmaq_laneq_f64(Clo, A2lo, Bhi, 0);
    Clo = vfmaq_laneq_f64(Clo, A3lo, Bhi, 1);

    float64x2_t Chi = vmovq_n_f64(0);
    Chi = vfmaq_laneq_f64(Chi, A0hi, Blo, 0);
    Chi = vfmaq_laneq_f64(Chi, A1hi, Blo, 1);
    Chi = vfmaq_laneq_f64(Chi, A2hi, Bhi, 0);
    Chi = vfmaq_laneq_f64(Chi, A3hi, Bhi, 1);

    vst1q_f64(&O[4*i], Clo);
    vst1q_f64(&O[4*i + 2], Chi);
  }
}

void DMat4xDVec4_SIMD(double *M, double *V, double *O) {
  float64x2_t M0lo = vld1q_f64(&M[0]);
  float64x2_t M0hi = vld1q_f64(&M[2]);
  float64x2_t M1lo = vld1q_f64(&M[4]);
  float64x2_t M1hi = vld1q_f64(&M[6]);
  float64x2_t M2lo = vld1q_f64(&M[8]);
  float64x2_t M2hi = vld1q_f64(&M[10]);
  float64x2_t M3lo = vld1q_f64(&M[12]);
  float64x2_t M3hi = vld1q_f64(&M[14]);

  float64x2_t Vlo = vld1q_f64(&V[0]);
  float64x2_t Vhi = vld1q_f64(&V[2]);

  float64x2_t Llo = vmovq_n_f64(0);
  Llo = vfmaq_laneq_f64(Llo, M0lo, Vlo, 0);
  Llo = vfmaq_laneq_f64(Llo, M1lo, Vlo, 1);
  Llo = vfmaq_laneq_f64(Llo, M2lo, Vhi, 0);
  Llo = vfmaq_laneq_f64(Llo, M3lo, Vhi, 1);

  float64x2_t Lhi = vmovq_n_f64(0);
  Lhi = vfmaq_laneq_f64(Lhi, M0hi, Vlo, 0);
  Lhi = vfmaq_laneq_f64(Lhi, M1hi, Vlo, 1);
  Lhi = vfmaq_laneq_f64(Lhi, M2hi, Vhi, 0);
  Lhi = vfmaq_laneq_f64(Lhi, M3hi, Vhi, 1);

  vst1q_f64(&O[0], Llo);
  vst1q_f64(&O[2], Lhi);
}

#else

void DMat4xDMat4_SIMD(double *A, double *B, double *O) {
  for(int i=0; i<4; i++) {
    for(int j=0; j<4; j++) {
      O[4*i + j] =
        (B[4*i + 0] * A[0 + j] + B[4*i + 1] * A[4 + j]) +
        (B[4*i + 2] * A[8 + j] + B[4*i + 3] * A[12 + j]);
    }
  }
}

void DMat4xDVec4_SIMD(double *M, double *V, double *O) {
  for(int j=0; j<4; j++) {
    O[j] =
      M[0 + j] * V[0] +
      M[4 + j] * V[1] +
      M[8 + j] * V[2] +
      M[12 + j] * V[3];
  }
}

#endif