#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "R_LeftHSAnchor.h"

#ifdef _MSC_VER
  #define INLINE __forceinline 
#else
  #define INLINE static inline        
#endif

INLINE double Power(double x, double y) { return pow(x, y); }
INLINE double Sqrt(double x) { return sqrt(x); }

INLINE double Abs(double x) { return fabs(x); }

INLINE double Exp(double x) { return exp(x); }
INLINE double Log(double x) { return log(x); }

INLINE double Sin(double x) { return sin(x); }
INLINE double Cos(double x) { return cos(x); }
INLINE double Tan(double x) { return tan(x); }

INLINE double Csc(double x) { return 1.0/sin(x); }
INLINE double Sec(double x) { return 1.0/cos(x); }

INLINE double ArcSin(double x) { return asin(x); }
INLINE double ArcCos(double x) { return acos(x); }

INLINE double ArcTan(double x, double y) { return atan2(y,x); }

INLINE double Sinh(double x) { return sinh(x); }
INLINE double Cosh(double x) { return cosh(x); }
INLINE double Tanh(double x) { return tanh(x); }

#define E 2.71828182845904523536029
#define Pi 3.14159265358979323846264
#define Degree 0.01745329251994329576924

static void output1(Eigen::Matrix<double,3,3> &p_output1, const Eigen::Matrix<double,36,1> &var1)
{
  double t1888;
  double t2505;
  double t1457;
  double t1608;
  double t1610;
  double t1858;
  double t2137;
  double t2298;
  double t2339;
  double t2419;
  double t2512;
  double t1220;
  double t3133;
  double t3137;
  double t3181;
  double t2435;
  double t3049;
  double t3109;
  double t3198;
  double t1218;
  double t3237;
  double t3238;
  double t3256;
  double t3116;
  double t3208;
  double t3213;
  double t3259;
  double t3281;
  double t3216;
  double t3260;
  double t3269;
  double t610;
  double t3282;
  double t3295;
  double t3313;
  double t3384;
  double t3272;
  double t3330;
  double t3336;
  double t393;
  double t3391;
  double t3410;
  double t3412;
  double t3457;
  double t3375;
  double t3416;
  double t3419;
  double t168;
  double t3469;
  double t3477;
  double t3480;
  double t47;
  double t3583;
  double t3602;
  double t3606;
  double t3658;
  double t3679;
  double t3694;
  double t3621;
  double t3630;
  double t3645;
  double t3723;
  double t3726;
  double t3728;
  double t3646;
  double t3695;
  double t3699;
  double t3712;
  double t3729;
  double t3736;
  double t3752;
  double t3760;
  double t3769;
  double t3747;
  double t3770;
  double t3778;
  double t3846;
  double t3856;
  double t3863;
  double t3516;
  double t3796;
  double t3867;
  double t3869;
  double t3878;
  double t3886;
  double t3896;
  double t4046;
  double t4056;
  double t4063;
  double t3997;
  double t4012;
  double t4022;
  double t4086;
  double t4097;
  double t4113;
  double t4036;
  double t4066;
  double t4079;
  double t4085;
  double t4138;
  double t4140;
  double t4164;
  double t4168;
  double t4196;
  double t4153;
  double t4199;
  double t4206;
  double t4227;
  double t4233;
  double t4240;
  double t4218;
  double t4249;
  double t4255;
  double t4263;
  double t4269;
  double t4275;
  double t3441;
  double t3499;
  double t3511;
  double t3521;
  double t3538;
  double t3545;
  double t3871;
  double t3897;
  double t3901;
  double t3906;
  double t3930;
  double t3960;
  double t4257;
  double t4286;
  double t4288;
  double t4292;
  double t4301;
  double t4302;
  t1888 = Cos(var1[3]);
  t2505 = Cos(var1[4]);
  t1457 = Cos(var1[6]);
  t1608 = Cos(var1[5]);
  t1610 = Sin(var1[3]);
  t1858 = -1.*t1608*t1610;
  t2137 = Sin(var1[4]);
  t2298 = Sin(var1[5]);
  t2339 = t1888*t2137*t2298;
  t2419 = t1858 + t2339;
  t2512 = Sin(var1[6]);
  t1220 = Cos(var1[7]);
  t3133 = t1888*t2505*t1457;
  t3137 = t2419*t2512;
  t3181 = t3133 + t3137;
  t2435 = t1457*t2419;
  t3049 = -1.*t1888*t2505*t2512;
  t3109 = t2435 + t3049;
  t3198 = Sin(var1[7]);
  t1218 = Cos(var1[8]);
  t3237 = t1220*t3181;
  t3238 = t3109*t3198;
  t3256 = t3237 + t3238;
  t3116 = t1220*t3109;
  t3208 = -1.*t3181*t3198;
  t3213 = t3116 + t3208;
  t3259 = Sin(var1[8]);
  t3281 = Cos(var1[9]);
  t3216 = t1218*t3213;
  t3260 = t3256*t3259;
  t3269 = t3216 + t3260;
  t610 = Sin(var1[9]);
  t3282 = t1218*t3256;
  t3295 = -1.*t3213*t3259;
  t3313 = t3282 + t3295;
  t3384 = Cos(var1[10]);
  t3272 = t610*t3269;
  t3330 = t3281*t3313;
  t3336 = t3272 + t3330;
  t393 = Sin(var1[10]);
  t3391 = t3281*t3269;
  t3410 = -1.*t610*t3313;
  t3412 = t3391 + t3410;
  t3457 = Cos(var1[11]);
  t3375 = -1.*t393*t3336;
  t3416 = t3384*t3412;
  t3419 = t3375 + t3416;
  t168 = Sin(var1[11]);
  t3469 = t3384*t3336;
  t3477 = t393*t3412;
  t3480 = t3469 + t3477;
  t47 = Cos(var1[12]);
  t3583 = t1888*t1608;
  t3602 = t1610*t2137*t2298;
  t3606 = t3583 + t3602;
  t3658 = t2505*t1457*t1610;
  t3679 = t3606*t2512;
  t3694 = t3658 + t3679;
  t3621 = t1457*t3606;
  t3630 = -1.*t2505*t1610*t2512;
  t3645 = t3621 + t3630;
  t3723 = t1220*t3694;
  t3726 = t3645*t3198;
  t3728 = t3723 + t3726;
  t3646 = t1220*t3645;
  t3695 = -1.*t3694*t3198;
  t3699 = t3646 + t3695;
  t3712 = t1218*t3699;
  t3729 = t3728*t3259;
  t3736 = t3712 + t3729;
  t3752 = t1218*t3728;
  t3760 = -1.*t3699*t3259;
  t3769 = t3752 + t3760;
  t3747 = t610*t3736;
  t3770 = t3281*t3769;
  t3778 = t3747 + t3770;
  t3846 = t3281*t3736;
  t3856 = -1.*t610*t3769;
  t3863 = t3846 + t3856;
  t3516 = Sin(var1[12]);
  t3796 = -1.*t393*t3778;
  t3867 = t3384*t3863;
  t3869 = t3796 + t3867;
  t3878 = t3384*t3778;
  t3886 = t393*t3863;
  t3896 = t3878 + t3886;
  t4046 = -1.*t1457*t2137;
  t4056 = t2505*t2298*t2512;
  t4063 = t4046 + t4056;
  t3997 = t2505*t1457*t2298;
  t4012 = t2137*t2512;
  t4022 = t3997 + t4012;
  t4086 = t1220*t4063;
  t4097 = t4022*t3198;
  t4113 = t4086 + t4097;
  t4036 = t1220*t4022;
  t4066 = -1.*t4063*t3198;
  t4079 = t4036 + t4066;
  t4085 = t1218*t4079;
  t4138 = t4113*t3259;
  t4140 = t4085 + t4138;
  t4164 = t1218*t4113;
  t4168 = -1.*t4079*t3259;
  t4196 = t4164 + t4168;
  t4153 = t610*t4140;
  t4199 = t3281*t4196;
  t4206 = t4153 + t4199;
  t4227 = t3281*t4140;
  t4233 = -1.*t610*t4196;
  t4240 = t4227 + t4233;
  t4218 = -1.*t393*t4206;
  t4249 = t3384*t4240;
  t4255 = t4218 + t4249;
  t4263 = t3384*t4206;
  t4269 = t393*t4240;
  t4275 = t4263 + t4269;
  t3441 = t168*t3419;
  t3499 = t3457*t3480;
  t3511 = t3441 + t3499;
  t3521 = t3457*t3419;
  t3538 = -1.*t168*t3480;
  t3545 = t3521 + t3538;
  t3871 = t168*t3869;
  t3897 = t3457*t3896;
  t3901 = t3871 + t3897;
  t3906 = t3457*t3869;
  t3930 = -1.*t168*t3896;
  t3960 = t3906 + t3930;
  t4257 = t168*t4255;
  t4286 = t3457*t4275;
  t4288 = t4257 + t4286;
  t4292 = t3457*t4255;
  t4301 = -1.*t168*t4275;
  t4302 = t4292 + t4301;

  p_output1(0)=t3516*t3545 + t3511*t47;
  p_output1(1)=t3516*t3960 + t3901*t47;
  p_output1(2)=t3516*t4302 + t4288*t47;
  p_output1(3)=-1.*t3511*t3516 + t3545*t47;
  p_output1(4)=-1.*t3516*t3901 + t3960*t47;
  p_output1(5)=-1.*t3516*t4288 + t4302*t47;
  p_output1(6)=t1608*t1888*t2137 + t1610*t2298;
  p_output1(7)=t1608*t1610*t2137 - 1.*t1888*t2298;
  p_output1(8)=t1608*t2505;
}

Eigen::Matrix<double,3,3> R_LeftHSAnchor(const Eigen::Matrix<double,36,1> &var1)

{

  Eigen::Matrix<double,3,3>  p_output1;

  output1(p_output1, var1);

  return p_output1;
}
/*
 * Automatically Generated from Mathematica.
 * Thu 4 Nov 2021 17:02:42 GMT-04:00
 */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "R_LeftHSAnchor.h"

#ifdef _MSC_VER
  #define INLINE __forceinline /* use __forceinline (VC++ specific) */
#else
  #define INLINE static inline        /* use standard inline */
#endif

/**
 * Copied from Wolfram Mathematica C Definitions file mdefs.hpp
 * Changed marcos to inline functions (Eric Cousineau)
 */
INLINE double Power(double x, double y) { return pow(x, y); }
INLINE double Sqrt(double x) { return sqrt(x); }

INLINE double Abs(double x) { return fabs(x); }

INLINE double Exp(double x) { return exp(x); }
INLINE double Log(double x) { return log(x); }

INLINE double Sin(double x) { return sin(x); }
INLINE double Cos(double x) { return cos(x); }
INLINE double Tan(double x) { return tan(x); }

INLINE double Csc(double x) { return 1.0/sin(x); }
INLINE double Sec(double x) { return 1.0/cos(x); }

INLINE double ArcSin(double x) { return asin(x); }
INLINE double ArcCos(double x) { return acos(x); }


/* update ArcTan function to use atan2 instead. */
INLINE double ArcTan(double x, double y) { return atan2(y,x); }

INLINE double Sinh(double x) { return sinh(x); }
INLINE double Cosh(double x) { return cosh(x); }
INLINE double Tanh(double x) { return tanh(x); }

#define E 2.71828182845904523536029
#define Pi 3.14159265358979323846264
#define Degree 0.01745329251994329576924

/*
 * Sub functions
 */
static void output1(Eigen::Matrix<double,3,3> &p_output1, const Eigen::Matrix<double,36,1> &var1)
{
  double t1888;
  double t2505;
  double t1457;
  double t1608;
  double t1610;
  double t1858;
  double t2137;
  double t2298;
  double t2339;
  double t2419;
  double t2512;
  double t1220;
  double t3133;
  double t3137;
  double t3181;
  double t2435;
  double t3049;
  double t3109;
  double t3198;
  double t1218;
  double t3237;
  double t3238;
  double t3256;
  double t3116;
  double t3208;
  double t3213;
  double t3259;
  double t3281;
  double t3216;
  double t3260;
  double t3269;
  double t610;
  double t3282;
  double t3295;
  double t3313;
  double t3384;
  double t3272;
  double t3330;
  double t3336;
  double t393;
  double t3391;
  double t3410;
  double t3412;
  double t3457;
  double t3375;
  double t3416;
  double t3419;
  double t168;
  double t3469;
  double t3477;
  double t3480;
  double t47;
  double t3583;
  double t3602;
  double t3606;
  double t3658;
  double t3679;
  double t3694;
  double t3621;
  double t3630;
  double t3645;
  double t3723;
  double t3726;
  double t3728;
  double t3646;
  double t3695;
  double t3699;
  double t3712;
  double t3729;
  double t3736;
  double t3752;
  double t3760;
  double t3769;
  double t3747;
  double t3770;
  double t3778;
  double t3846;
  double t3856;
  double t3863;
  double t3516;
  double t3796;
  double t3867;
  double t3869;
  double t3878;
  double t3886;
  double t3896;
  double t4046;
  double t4056;
  double t4063;
  double t3997;
  double t4012;
  double t4022;
  double t4086;
  double t4097;
  double t4113;
  double t4036;
  double t4066;
  double t4079;
  double t4085;
  double t4138;
  double t4140;
  double t4164;
  double t4168;
  double t4196;
  double t4153;
  double t4199;
  double t4206;
  double t4227;
  double t4233;
  double t4240;
  double t4218;
  double t4249;
  double t4255;
  double t4263;
  double t4269;
  double t4275;
  double t3441;
  double t3499;
  double t3511;
  double t3521;
  double t3538;
  double t3545;
  double t3871;
  double t3897;
  double t3901;
  double t3906;
  double t3930;
  double t3960;
  double t4257;
  double t4286;
  double t4288;
  double t4292;
  double t4301;
  double t4302;
  t1888 = Cos(var1[3]);
  t2505 = Cos(var1[4]);
  t1457 = Cos(var1[6]);
  t1608 = Cos(var1[5]);
  t1610 = Sin(var1[3]);
  t1858 = -1.*t1608*t1610;
  t2137 = Sin(var1[4]);
  t2298 = Sin(var1[5]);
  t2339 = t1888*t2137*t2298;
  t2419 = t1858 + t2339;
  t2512 = Sin(var1[6]);
  t1220 = Cos(var1[7]);
  t3133 = t1888*t2505*t1457;
  t3137 = t2419*t2512;
  t3181 = t3133 + t3137;
  t2435 = t1457*t2419;
  t3049 = -1.*t1888*t2505*t2512;
  t3109 = t2435 + t3049;
  t3198 = Sin(var1[7]);
  t1218 = Cos(var1[8]);
  t3237 = t1220*t3181;
  t3238 = t3109*t3198;
  t3256 = t3237 + t3238;
  t3116 = t1220*t3109;
  t3208 = -1.*t3181*t3198;
  t3213 = t3116 + t3208;
  t3259 = Sin(var1[8]);
  t3281 = Cos(var1[9]);
  t3216 = t1218*t3213;
  t3260 = t3256*t3259;
  t3269 = t3216 + t3260;
  t610 = Sin(var1[9]);
  t3282 = t1218*t3256;
  t3295 = -1.*t3213*t3259;
  t3313 = t3282 + t3295;
  t3384 = Cos(var1[10]);
  t3272 = t610*t3269;
  t3330 = t3281*t3313;
  t3336 = t3272 + t3330;
  t393 = Sin(var1[10]);
  t3391 = t3281*t3269;
  t3410 = -1.*t610*t3313;
  t3412 = t3391 + t3410;
  t3457 = Cos(var1[11]);
  t3375 = -1.*t393*t3336;
  t3416 = t3384*t3412;
  t3419 = t3375 + t3416;
  t168 = Sin(var1[11]);
  t3469 = t3384*t3336;
  t3477 = t393*t3412;
  t3480 = t3469 + t3477;
  t47 = Cos(var1[12]);
  t3583 = t1888*t1608;
  t3602 = t1610*t2137*t2298;
  t3606 = t3583 + t3602;
  t3658 = t2505*t1457*t1610;
  t3679 = t3606*t2512;
  t3694 = t3658 + t3679;
  t3621 = t1457*t3606;
  t3630 = -1.*t2505*t1610*t2512;
  t3645 = t3621 + t3630;
  t3723 = t1220*t3694;
  t3726 = t3645*t3198;
  t3728 = t3723 + t3726;
  t3646 = t1220*t3645;
  t3695 = -1.*t3694*t3198;
  t3699 = t3646 + t3695;
  t3712 = t1218*t3699;
  t3729 = t3728*t3259;
  t3736 = t3712 + t3729;
  t3752 = t1218*t3728;
  t3760 = -1.*t3699*t3259;
  t3769 = t3752 + t3760;
  t3747 = t610*t3736;
  t3770 = t3281*t3769;
  t3778 = t3747 + t3770;
  t3846 = t3281*t3736;
  t3856 = -1.*t610*t3769;
  t3863 = t3846 + t3856;
  t3516 = Sin(var1[12]);
  t3796 = -1.*t393*t3778;
  t3867 = t3384*t3863;
  t3869 = t3796 + t3867;
  t3878 = t3384*t3778;
  t3886 = t393*t3863;
  t3896 = t3878 + t3886;
  t4046 = -1.*t1457*t2137;
  t4056 = t2505*t2298*t2512;
  t4063 = t4046 + t4056;
  t3997 = t2505*t1457*t2298;
  t4012 = t2137*t2512;
  t4022 = t3997 + t4012;
  t4086 = t1220*t4063;
  t4097 = t4022*t3198;
  t4113 = t4086 + t4097;
  t4036 = t1220*t4022;
  t4066 = -1.*t4063*t3198;
  t4079 = t4036 + t4066;
  t4085 = t1218*t4079;
  t4138 = t4113*t3259;
  t4140 = t4085 + t4138;
  t4164 = t1218*t4113;
  t4168 = -1.*t4079*t3259;
  t4196 = t4164 + t4168;
  t4153 = t610*t4140;
  t4199 = t3281*t4196;
  t4206 = t4153 + t4199;
  t4227 = t3281*t4140;
  t4233 = -1.*t610*t4196;
  t4240 = t4227 + t4233;
  t4218 = -1.*t393*t4206;
  t4249 = t3384*t4240;
  t4255 = t4218 + t4249;
  t4263 = t3384*t4206;
  t4269 = t393*t4240;
  t4275 = t4263 + t4269;
  t3441 = t168*t3419;
  t3499 = t3457*t3480;
  t3511 = t3441 + t3499;
  t3521 = t3457*t3419;
  t3538 = -1.*t168*t3480;
  t3545 = t3521 + t3538;
  t3871 = t168*t3869;
  t3897 = t3457*t3896;
  t3901 = t3871 + t3897;
  t3906 = t3457*t3869;
  t3930 = -1.*t168*t3896;
  t3960 = t3906 + t3930;
  t4257 = t168*t4255;
  t4286 = t3457*t4275;
  t4288 = t4257 + t4286;
  t4292 = t3457*t4255;
  t4301 = -1.*t168*t4275;
  t4302 = t4292 + t4301;

  p_output1(0)=t3516*t3545 + t3511*t47;
  p_output1(1)=t3516*t3960 + t3901*t47;
  p_output1(2)=t3516*t4302 + t4288*t47;
  p_output1(3)=-1.*t3511*t3516 + t3545*t47;
  p_output1(4)=-1.*t3516*t3901 + t3960*t47;
  p_output1(5)=-1.*t3516*t4288 + t4302*t47;
  p_output1(6)=t1608*t1888*t2137 + t1610*t2298;
  p_output1(7)=t1608*t1610*t2137 - 1.*t1888*t2298;
  p_output1(8)=t1608*t2505;
}


       
Eigen::Matrix<double,3,3> R_LeftHSAnchor(const Eigen::Matrix<double,36,1> &var1)
//void R_LeftHSAnchor(Eigen::Matrix<double,3,3> &p_output1, const Eigen::Matrix<double,36,1> &var1)
{
  /* Call Subroutines */
  Eigen::Matrix<double,3,3>  p_output1;
  
  output1(p_output1, var1);

  return p_output1;
}



