FasterCard/ProxyFunctions.cpp at main · cesare-corrado/FasterCard · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
#include <math.h>
#include<vector>
#include "ProxyFunctions.hpp"

std::vector<double> InvertA3X3(const std::vector<double> & Mat0)
{
  //matrix is considered as row major
  std::vector<double> inverted(9,0);
  double det =  DeterA3X3(Mat0);
  if(sqrt(det*det)>0.0)
  {
    inverted[RMIndex(0,0,3)] =        Mat0[RMIndex(1,1,3)]*Mat0[RMIndex(2,2,3)] - Mat0[RMIndex(1,2,3)]*Mat0[RMIndex(2,1,3)];
    inverted[RMIndex(0,1,3)] = -1.0*( Mat0[RMIndex(0,1,3)]*Mat0[RMIndex(2,2,3)] - Mat0[RMIndex(2,1,3)]*Mat0[RMIndex(0,2,3)]);
    inverted[RMIndex(0,2,3)] =        Mat0[RMIndex(0,1,3)]*Mat0[RMIndex(1,2,3)] - Mat0[RMIndex(0,2,3)]*Mat0[RMIndex(1,1,3)];

    inverted[RMIndex(1,0,3)] = -1.0*( Mat0[RMIndex(1,0,3)]*Mat0[RMIndex(2,2,3)] - Mat0[RMIndex(1,2,3)]*Mat0[RMIndex(2,0,3)] );
    inverted[RMIndex(1,1,3)] =        Mat0[RMIndex(0,0,3)]*Mat0[RMIndex(2,2,3)] - Mat0[RMIndex(0,2,3)]*Mat0[RMIndex(2,0,3)];
    inverted[RMIndex(1,2,3)] = -1.0*( Mat0[RMIndex(0,0,3)]*Mat0[RMIndex(1,2,3)] - Mat0[RMIndex(0,2,3)]*Mat0[RMIndex(1,0,3)]);

    inverted[RMIndex(2,0,3)] =        Mat0[RMIndex(1,0,3)]*Mat0[RMIndex(2,1,3)] - Mat0[RMIndex(2,0,3)]*Mat0[RMIndex(1,1,3)];
    inverted[RMIndex(2,1,3)] = -1.0*( Mat0[RMIndex(0,0,3)]*Mat0[RMIndex(2,1,3)] - Mat0[RMIndex(2,0,3)]*Mat0[RMIndex(0,1,3)]);
    inverted[RMIndex(2,2,3)] =        Mat0[RMIndex(1,1,3)]*Mat0[RMIndex(0,0,3)] - Mat0[RMIndex(1,0,3)]*Mat0[RMIndex(0,1,3)];

    std::vector<double>::iterator it;
    for(it=inverted.begin(); it!=inverted.end(); ++it)
    {
      *it=*it/det;
    }
  }
  else
  {
    inverted.clear();
    inverted.resize(9,0);
  }
  return(inverted);
}