generate_Steiner_ellipse_3d.py

Go to the documentation of this file.

from __future__ import print_function
from sympy import *
import sys

# Lets work this out symbolically first and perform python unit test.

# From http://en.wikipedia.org/wiki/Steiner_ellipse
x1=symbols('x1[:3]')
x2=symbols('x2[:3]')
x3=symbols('x3[:3]')
x4=symbols('x4[:3]')

Mij=symbols('Mij[:36]')

M = Matrix([
    [(x1[0] - x2[0])**2, (x1[1] - x2[1])**2, (x1[2] - x2[2])**2, (x1[1] - x2[1])*(x1[2] - x2[2]), (x1[0] - x2[0])*(x1[2] - x2[2]), (x1[0] - x2[0])*(x1[1] - x2[1])],
    [(x2[0] - x3[0])**2, (x2[1] - x3[1])**2, (x2[2] - x3[2])**2, (x2[1] - x3[1])*(x2[2] - x3[2]), (x2[0] - x3[0])*(x2[2] - x3[2]), (x2[0] - x3[0])*(x2[1] - x3[1])],
    [(x3[0] - x1[0])**2, (x3[1] - x1[1])**2, (x3[2] - x1[2])**2, (x3[1] - x1[1])*(x3[2] - x1[2]), (x3[0] - x1[0])*(x3[2] - x1[2]), (x3[0] - x1[0])*(x3[1] - x1[1])],
    [(x1[0] - x4[0])**2, (x1[1] - x4[1])**2, (x1[2] - x4[2])**2, (x1[1] - x4[1])*(x1[2] - x4[2]), (x1[0] - x4[0])*(x1[2] - x4[2]), (x1[0] - x4[0])*(x1[1] - x4[1])],
    [(x2[0] - x4[0])**2, (x2[1] - x4[1])**2, (x2[2] - x4[2])**2, (x2[1] - x4[1])*(x2[2] - x4[2]), (x2[0] - x4[0])*(x2[2] - x4[2]), (x2[0] - x4[0])*(x2[1] - x4[1])],
    [(x3[0] - x4[0])**2, (x3[1] - x4[1])**2, (x3[2] - x4[2])**2, (x3[1] - x4[1])*(x3[2] - x4[2]), (x3[0] - x4[0])*(x3[2] - x4[2]), (x3[0] - x4[0])*(x3[1] - x4[1])]])

R=Matrix([[1], [1], [1], [1], [1], [1]])

# http://en.wikipedia.org/wiki/Tetrahedron#Formulas_for_a_regular_tetrahedron
tetrahedron = {x1[0]:1,  x1[1]:0,  x1[2]:-4/sqrt(2),
               x2[0]:-1, x2[1]:0,  x2[2]:-4/sqrt(2),
               x3[0]:0,  x3[1]:2,  x3[2]:4/sqrt(2), 
               x4[0]:0,  x4[1]:-2, x4[2]:4/sqrt(2)}

Mi = M.evalf(subs=tetrahedron)

Sxx, Syy, Szz, Syz, Sxz, Sxy = Mi.inv()*R

SteinerEllipse = Matrix([
    [Sxx, Sxy, Sxz],
    [Sxy, Syy, Syz],
    [Sxz, Syz, Szz]])

print("SteinerEllipse = ")
pprint(SteinerEllipse)

if Sxx==1./2**2 and Syy==1./4**2 and Szz==1./8**2 and Syz==0.0 and Sxz==0.0 and Sxy==0.0:
    print("pass")
else:
    print("fail")
    sys.exit(-1)

# Move onto code generation.

pyname=sys.argv[0].split('/')[-1]

# Write header file
hname=pyname[:-3]+".h"
macro = pyname[:-3].upper()+"_H"
header="""/* Start of code generated by %s. Warning - be careful about modifying
any of the generated code directly.  Any changes/fixes should be done
in the code generation script generation.*/\n

#ifndef %s
#define %s

namespace pragmatic
{

void generate_Steiner_ellipse(const double *x1, const double *x2, const double *x3, const double *x4, double *sm);

}

#endif
"""%(pyname, macro, macro)

hfile = open(hname, 'w')
hfile.write(header)
hfile.close()

# Write source file
cxxname=pyname[:-3]+".cpp"

src="""
/* Start of code generated by %s. Warning - be careful about modifying
   any of the generated code directly.  Any changes/fixes should be done
   in the code generation script generation.
 */

#include <Eigen/Core>
#include <Eigen/Dense>

#include <%s>

"""%(pyname, hname)

src += """
void pragmatic::generate_Steiner_ellipse(const double *x1, const double *x2, const double *x3, const double *x4, double *sm){
  // # From http://en.wikipedia.org/wiki/Steiner_ellipse

  Eigen::Matrix<double, 6, 6> M;

  M <<"""
for i in range(6):
    for j in range(6):
        src+=ccode(M[i,j], contract=False)
        if not (i==5 and j==5):
            src+=", "
    if i==5:
        src+=";\n"
    else:
        src+="\n"
src+="""
  Eigen::Matrix<double, 6, 1> R;
  R<<1,1,1,1,1,1;
  Eigen::Matrix<double, 6, 1> S;

  M.svd().solve(R, &S);

  sm[0] = S[0]; sm[1] = S[5]; sm[2] = S[4];
                sm[3] = S[1]; sm[4] = S[3];
                              sm[5] = S[2];
  return;
}

/* End of code generated by %s. Warning - be careful about
   modifying any of the generated code directly.  Any changes/fixes
   should be done in the code generation script generation.*/\n"""%pyname

cxxfile = open(cxxname, "w")
cxxfile.write(src)
cxxfile.close()

# Write unit test code.
testname="test_"+pyname[:-3]+".cpp"
testsrc="""
#include <cmath>
#include <cfloat>
#include <iostream>

#include <%s>

int main(){
  double x1[]={ 1,  0, -4/sqrt(2)};
  double x2[]={-1,  0, -4/sqrt(2)};
  double x3[]={ 0,  2,  4/sqrt(2)};  
  double x4[]={ 0, -2,  4/sqrt(2)};
  double sm[6];
  pragmatic::generate_Steiner_ellipse(x1, x2, x3, x4, sm);

  // Test
  if(fabs(sm[0]-1./sqrt(2))<DBL_EPSILON && fabs(sm[1])<DBL_EPSILON            && fabs(sm[2])<DBL_EPSILON &&
                                           fabs(sm[3]-1./sqrt(4))<DBL_EPSILON && fabs(sm[4])<DBL_EPSILON &&
                                                                                 fabs(sm[5]-1./sqrt(8))<DBL_EPSILON){
    std::cout<<"pass"<<std::endl;
  }else{
    std::cout<<"fail"<<std::endl;
  }

  return 0;
}
"""%(hname)

testfile = open(testname, "w")
testfile.write(testsrc)
testfile.close()