Merge branch 'darwin_libs' into 'master'

Moved the darwin math and kinematics modules to the darwin firmware repo.

See merge request !11

dynamixel_manager/Makefile

@@ -19,7 +19,7 @@ SCHEDULER_PATH = ../scheduler
 
 INCLUDE_DIRS = -I./include/ -I./include/modules -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include -I$(MEMORY_PATH)/include -I$(SCHEDULER_PATH)/include
 
-MACROS = -DMAX_DYN_MASTER_TX_BUFFER_LEN=256 -DMAX_DYN_MASTER_RX_BUFFER_LEN=256 -DDYN_MANAGER_MAX_NUM_MASTERS=4 -DDYN_MANAGER_MAX_NUM_MODULES=8 -DDYN_MANAGER_MAX_NUM_DEVICES=32 -DDYN_MANAGER_MAX_NUM_SINGLE_OP=16 -DDYN_MANAGER_MAX_NUM_SYNC_OP=4 -DDYN_MANAGER_MAX_NUM_BULK_OP=4 -DMODULE_MAX_NUM_MODELS=32 -DMM_MAX_NUM_MOTION_MODULES=8 -DNUM_MOTION_PAGES=256
+MACROS = -DMAX_DYN_MASTER_TX_BUFFER_LEN=256 -DMAX_DYN_MASTER_RX_BUFFER_LEN=256 -DDYN_MANAGER_MAX_NUM_MASTERS=4 -DDYN_MANAGER_MAX_NUM_MODULES=8 -DDYN_MANAGER_MAX_NUM_DEVICES=32 -DDYN_MANAGER_MAX_NUM_SINGLE_OP=16 -DDYN_MANAGER_MAX_NUM_SYNC_OP=4 -DDYN_MANAGER_MAX_NUM_BULK_OP=4 -DMODULE_MAX_NUM_MODELS=32 -DMM_MAX_NUM_MOTION_MODULES=8 -DNUM_MOTION_PAGES=256 -DNUM_JOINT_GROUPS=4
 
 TCHAIN_PREFIX=arm-none-eabi-
 

dynamixel_manager/include/modules/action.h

 #ifndef _ACTION_H
 #define _ACTION_H
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
 #include "motion_pages.h"
 #include "motion_module.h"
 #include "memory.h"
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 typedef enum {ACTION_PRE,ACTION_MAIN,ACTION_POST,ACTION_PAUSE} action_states;
 
 typedef struct{

dynamixel_manager/include/modules/head_tracking.h

 #ifndef _HEAD_TRACKING_H
 #define _HEAD_TRACKINH_H
 
+#include "head_tracking_registers.h"
+#include "motion_module.h"
+#include "memory.h"
+
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-#include "motion_module.h"
-#include "memory.h"
-
 typedef struct{
   unsigned short int kp;
   unsigned short int ki;
@@ -37,6 +38,7 @@ typedef struct{
 
 // public functions
 unsigned char head_tracking_init(THeadMModule *joint,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address);
+TMotionModule *head_tracking_get_module(THeadMModule *head);
 void head_tracking_start(THeadMModule *joint);
 void head_tracking_stop(THeadMModule *joint);
 unsigned char head_is_tracking(THeadMModule *joint);

dynamixel_manager/include/modules/head_tracking_registers.h

@@ -45,7 +45,7 @@ unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))
   DEFAULT_TILT_D_GAIN&0x00FF,base_address+HEAD_TRACKING_TILT_D_GAIN_OFFSET, \
   (DEFAULT_TILT_D_GAIN>>8)&0x00FF,base_address+HEAD_TRACKING_TILT_D_GAIN_OFFSET+1, \
   DEFAULT_TILT_I_CLAMP&0x00FF,base_address+HEAD_TRACKING_TILT_I_CLAMP_OFFSET, \
-  (DEFAULT_TILT_I_CLAMP>>8)&0x00FF,base_address+HEAD_TRACKING_TILT_I_CLAMP_OFFSET+1 \
+  (DEFAULT_TILT_I_CLAMP>>8)&0x00FF,base_address+HEAD_TRACKING_TILT_I_CLAMP_OFFSET+1, \
   DEFAULT_PAN_SERVO_ID&0x00FF,base_address+HEAD_TRACKING_PAN_SERVO_ID_OFFSET, \
   DEFAULT_TILT_SERVO_ID&0x00FF,base_address+HEAD_TRACKING_TILT_SERVO_ID_OFFSET};
 

dynamixel_manager/include/modules/joint_motion.h

 #ifndef _JOINT_MOTION_H
 #define _JOINT_MOTION_H
 
+#include "joint_motion_registers.h"
+#include "motion_module.h"
+#include "memory.h"
+
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-#include "motion_module.h"
-#include "memory.h"
-
-#define NUM_JOINT_GROUPS 4
+#ifndef NUM_JOINT_GROUPS
+  #error "Please, specify the maximum number of joint groups with the NUM_JOINT_GROUPS macro"
+#endif
 
 typedef struct{
   TMotionModule mmodule;
@@ -31,6 +34,7 @@ typedef struct{
 
 // public functions
 unsigned char joint_motion_init(TJointMModule *joint,TMemory *memory,unsigned short int ram_base_address);
+TMotionModule *joint_motion_get_module(TJointMModule *action);
 void joint_motion_set_group_servos(TJointMModule *joint,unsigned char group_id,unsigned int servos);
 unsigned int joint_motion_get_group_servos(TJointMModule *joint,unsigned char group_id);
 void joint_motion_start(TJointMModule *joint,unsigned char group_id);

dynamixel_manager/include/modules/old/darwin_kinematics.h

-#ifndef _DARWIN_KINEMATICS_H
-#define _DARWIN_KINEMATICS_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-
-#define DARWIN_PELVIS_LENGTH               29.0 //mm
-#define DARWIN_LEG_SIDE_OFFSET             37.0 //mm
-#define DARWIN_THIGH_LENGTH                93.0 //mm
-#define DARWIN_CALF_LENGTH                 93.0 //mm
-#define DARWIN_ANKLE_LENGTH                33.5 //mm
-#define DARWIN_LEG_LENGTH                  (DARWIN_THIGH_LENGTH + DARWIN_CALF_LENGTH + DARWIN_ANKLE_LENGTH) //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH)
-
-// public functions
-uint8_t darwin_leg_ik(float *out, float x, float y, float z, float a, float b, float c);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif

dynamixel_manager/include/modules/old/darwin_math.h

-#ifndef _DARWIN_MATH_H
-#define _DARWIN_MATH_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-
-#define   PI    3.14159
-
-typedef struct{
-  float x;
-  float y;
-  float z;
-}TPoint3D;
-
-// point public functions
-void point3d_init(TPoint3D *point);
-void point3d_load(TPoint3D *point, float x, float y, float z);
-void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src);
-
-typedef struct {
-  float x;
-  float y;
-  float z;
-}TVector3D;
-
-// vector public functions
-void vector3d_init(TVector3D *vector);
-void vector3d_load(TVector3D *vector, float x, float y, float z);
-void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src);
-float vector3d_length(TVector3D *vector);
-
-enum{
-  m00=0,
-  m01,
-  m02,
-  m03,
-  m10,
-  m11,
-  m12,
-  m13,
-  m20,
-  m21,
-  m22,
-  m23,
-  m30,
-  m31,
-  m32,
-  m33,
-};
-
-typedef struct{
-  float coef[16];
-}TMatrix3D;
-
-// matrix public functions
-void matrix3d_init(TMatrix3D *matrix);
-void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src);
-void matrix3d_identity(TMatrix3D *matrix);
-uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv);
-void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector);
-void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif

dynamixel_manager/include/modules/walk.h

@@ -5,6 +5,7 @@
 extern "C" {
 #endif
 
+#include "walk_registers.h"
 #include "motion_module.h"
 #include "memory.h"
 
@@ -88,6 +89,7 @@ typedef struct{
  
 // public functions
 unsigned char walk_init(TWalkMModule *walk,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address);
+TMotionModule *walk_get_module(TWalkMModule *walk);
 void walk_start(TWalkMModule *walk);
 void walk_stop(TWalkMModule *walk);
 unsigned char is_walk(TWalkMModule *walk);

dynamixel_manager/include/modules/walk_registers.h

@@ -68,20 +68,20 @@ unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))
   DEFAULT_PELVIS_OFFSET,base_address+WALK_PELVIS_OFFSET, \
   DEFAULT_ARM_SWING_GAIN,base_address+WALK_ARM_SWING_GAIN, \
   DEFAULT_MAX_VEL,base_address+WALK_MAX_VEL, \
-  DEFAULT_MAX_ROT_VEL,base_address+WALK_MAX_ROT_VEL \
-  DEFAULT_R_HIP_YAW_SERVO_ID,base_address+WALK_R_HIP_YAW_SERVO_ID \
-  DEFAULT_R_HIP_ROLL_SERVO_ID,base_address+WALK_R_HIP_ROLL_SERVO_ID \
-  DEFAULT_R_HIP_PITCH_SERVO_ID,base_address+WALK_R_HIP_PITCH_SERVO_ID \
-  DEFAULT_R_KNEE_SERVO_ID,base_address+WALK_R_KNEE_SERVO_ID \
-  DEFAULT_R_ANKLE_PITCH_SERVO_ID,base_address+WALK_R_ANKLE_PITCH_SERVO_ID \
-  DEFAULT_R_ANKLE_ROLL_SERVO_ID,base_address+WALK_R_ANKLE_ROLL_SERVO_ID \
-  DEFAULT_R_SHOULDER_PITCH_SERVO_ID,base_address+WALK_R_SHOULDER_PITCH_SERVO_ID \
-  DEFAULT_L_HIP_YAW_SERVO_ID,base_address+WALK_L_HIP_YAW_SERVO_ID \
-  DEFAULT_L_HIP_ROLL_SERVO_ID,base_address+WALK_L_HIP_ROLL_SERVO_ID \
-  DEFAULT_L_HIP_PITCH_SERVO_ID,base_address+WALK_L_HIP_PITCH_SERVO_ID \
-  DEFAULT_L_KNEE_SERVO_ID,base_address+WALK_L_KNEE_SERVO_ID \
-  DEFAULT_L_ANKLE_PITCH_SERVO_ID,base_address+WALK_L_ANKLE_PITCH_SERVO_ID \
-  DEFAULT_L_ANKLE_ROLL_SERVO_ID,base_address+WALK_L_ANKLE_ROLL_SERVO_ID \
+  DEFAULT_MAX_ROT_VEL,base_address+WALK_MAX_ROT_VEL, \
+  DEFAULT_R_HIP_YAW_SERVO_ID,base_address+WALK_R_HIP_YAW_SERVO_ID, \
+  DEFAULT_R_HIP_ROLL_SERVO_ID,base_address+WALK_R_HIP_ROLL_SERVO_ID, \
+  DEFAULT_R_HIP_PITCH_SERVO_ID,base_address+WALK_R_HIP_PITCH_SERVO_ID, \
+  DEFAULT_R_KNEE_SERVO_ID,base_address+WALK_R_KNEE_SERVO_ID, \
+  DEFAULT_R_ANKLE_PITCH_SERVO_ID,base_address+WALK_R_ANKLE_PITCH_SERVO_ID, \
+  DEFAULT_R_ANKLE_ROLL_SERVO_ID,base_address+WALK_R_ANKLE_ROLL_SERVO_ID, \
+  DEFAULT_R_SHOULDER_PITCH_SERVO_ID,base_address+WALK_R_SHOULDER_PITCH_SERVO_ID, \
+  DEFAULT_L_HIP_YAW_SERVO_ID,base_address+WALK_L_HIP_YAW_SERVO_ID, \
+  DEFAULT_L_HIP_ROLL_SERVO_ID,base_address+WALK_L_HIP_ROLL_SERVO_ID, \
+  DEFAULT_L_HIP_PITCH_SERVO_ID,base_address+WALK_L_HIP_PITCH_SERVO_ID, \
+  DEFAULT_L_KNEE_SERVO_ID,base_address+WALK_L_KNEE_SERVO_ID, \
+  DEFAULT_L_ANKLE_PITCH_SERVO_ID,base_address+WALK_L_ANKLE_PITCH_SERVO_ID, \
+  DEFAULT_L_ANKLE_ROLL_SERVO_ID,base_address+WALK_L_ANKLE_ROLL_SERVO_ID, \
   DEFAULT_L_SHOULDER_PITCH_SERVO_ID,base_address+WALK_L_SHOULDER_PITCH_SERVO_ID};
 
 #endif

dynamixel_manager/src/modules/old/darwin_kinematics.c

-#include "darwin_kinematics.h"
-#include "darwin_math.h"
-#include <math.h>
-
-uint8_t darwin_leg_ik(float *out, float x, float y, float z, float a, float b, float c)
-{
-  TMatrix3D Tad, Tda, Tcd, Tdc, Tac;
-  TVector3D vec,orientation;
-  TPoint3D position;
-  float _Rac, _Acos, _Atan, _k, _l, _m, _n, _s, _c, _theta;
-
-  vector3d_load(&orientation,a * 180.0 / PI, b * 180.0 / PI, c * 180.0 / PI);
-  point3d_load(&position,x, y, z - DARWIN_LEG_LENGTH);
-  matrix3d_set_transform(&Tad,&position,&orientation);
-
-  vec.x = x + Tad.coef[2] * DARWIN_ANKLE_LENGTH;
-  vec.y = y + Tad.coef[6] * DARWIN_ANKLE_LENGTH;
-  vec.z = (z - DARWIN_LEG_LENGTH) + Tad.coef[10] * DARWIN_ANKLE_LENGTH;
-
-  // Get Knee
-  _Rac = vector3d_length(&vec);
-  _Acos = acos((_Rac * _Rac - DARWIN_THIGH_LENGTH * DARWIN_THIGH_LENGTH - DARWIN_CALF_LENGTH * DARWIN_CALF_LENGTH) / (2 * DARWIN_THIGH_LENGTH * DARWIN_CALF_LENGTH));
-  if(isnan(_Acos) == 1)
-    return 0x00;;
-  *(out + 3) = _Acos;
-
-  // Get Ankle Roll
-  Tda = Tad;
-  if(matrix3d_inverse(&Tda,&Tda) == 0x00)
-    return 0x00;
-  _k = sqrt(Tda.coef[7] * Tda.coef[7] + Tda.coef[11] * Tda.coef[11]);
-  _l = sqrt(Tda.coef[7] * Tda.coef[7] + (Tda.coef[11] - DARWIN_ANKLE_LENGTH) * (Tda.coef[11] - DARWIN_ANKLE_LENGTH));
-  _m = (_k * _k - _l * _l - DARWIN_ANKLE_LENGTH * DARWIN_ANKLE_LENGTH) / (2 * _l * DARWIN_ANKLE_LENGTH);
-  if(_m > 1.0)
-    _m = 1.0;
-  else if(_m < -1.0)
-    _m = -1.0;
-  _Acos = acos(_m);
-  if(isnan(_Acos) == 1)
-    return 0x00;
-  if(Tda.coef[7] < 0.0)
-    *(out + 5) = -_Acos;
-  else
-    *(out + 5) = _Acos;
-  // Get Hip Yaw
-  vector3d_load(&orientation,*(out + 5) * 180.0 / PI, 0, 0);
-  point3d_load(&position,0, 0, -DARWIN_ANKLE_LENGTH);
-  matrix3d_set_transform(&Tcd,&position,&orientation);
-  Tdc = Tcd;
-  if(matrix3d_inverse(&Tdc,&Tdc) == 0x00)
-    return 0x00;
-  matrix3d_mult(&Tac,&Tad,&Tdc);
-  _Atan = atan2(-Tac.coef[1] , Tac.coef[5]);
-  if(isinf(_Atan) == 1)
-    return 0x00;
-  *(out) = _Atan;
-
-  // Get Hip Roll
-  _Atan = atan2(Tac.coef[9], -Tac.coef[1] * sin(*(out)) + Tac.coef[5] * cos(*(out)));
-  if(isinf(_Atan) == 1)
-    return 0x00;
-  *(out + 1) = _Atan;
-
-  // Get Hip Pitch and Ankle Pitch
-  _Atan = atan2(Tac.coef[2] * cos(*(out)) + Tac.coef[6] * sin(*(out)), Tac.coef[0] * cos(*(out)) + Tac.coef[4] * sin(*(out)));
-  if(isinf(_Atan) == 1)
-    return 0x00;
-  _theta = _Atan;
-  _k = sin(*(out + 3)) * DARWIN_CALF_LENGTH;
-  _l = -DARWIN_THIGH_LENGTH - cos(*(out + 3)) * DARWIN_CALF_LENGTH;
-  _m = cos(*(out)) * vec.x + sin(*(out)) * vec.y;
-  _n = cos(*(out + 1)) * vec.z + sin(*(out)) * sin(*(out + 1)) * vec.x - cos(*(out)) * sin(*(out + 1)) * vec.y;
-  _s = (_k * _n + _l * _m) / (_k * _k + _l * _l);
-  _c = (_n - _k * _s) / _l;
-  _Atan = atan2(_s, _c);
-  if(isinf(_Atan) == 1)
-    return 0x00;
-  *(out + 2) = _Atan;
-  *(out + 4) = _theta - *(out + 3) - *(out + 2);
-
-  return 0x01;
-}

dynamixel_manager/src/modules/old/darwin_math.c

-#include "darwin_math.h"
-#include <math.h>
-
-// point functions
-void point3d_init(TPoint3D *point)
-{
-  point->x=0.0;
-  point->y=0.0;
-  point->z=0.0;
-}
-
-void point3d_load(TPoint3D *point, float x, float y,float z)
-{
-  point->x=x;
-  point->y=y;
-  point->z=z;  
-}
-
-void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src)
-{
-  point_dst->x=point_src->x;
-  point_dst->y=point_src->y;
-  point_dst->z=point_src->z;
-}
-
-// vector functions
-void vector3d_init(TVector3D *vector)
-{
-  vector->x=0.0;
-  vector->y=0.0;
-  vector->z=0.0;
-}
-
-void vector3d_load(TVector3D *vector, float x, float y, float z)
-{
-  vector->x=x;
-  vector->y=y;
-  vector->z=z;
-}
-
-void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src)
-{
-  vector_dst->x=vector_src->x;
-  vector_dst->y=vector_src->y;
-  vector_dst->z=vector_src->z;
-}
-
-float vector3d_length(TVector3D *vector)
-{
-  return sqrt(vector->x*vector->x + vector->y*vector->y + vector->z*vector->z); 
-}
-
-// matrix functions
-void matrix3d_init(TMatrix3D *matrix)
-{
-   matrix3d_identity(matrix);
-}
-
-void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src)
-{
-  matrix_dst->coef[m00]=matrix_src->coef[m00];
-  matrix_dst->coef[m01]=matrix_src->coef[m01];
-  matrix_dst->coef[m02]=matrix_src->coef[m02];
-  matrix_dst->coef[m03]=matrix_src->coef[m03];
-  matrix_dst->coef[m10]=matrix_src->coef[m10];
-  matrix_dst->coef[m11]=matrix_src->coef[m11];
-  matrix_dst->coef[m12]=matrix_src->coef[m12];
-  matrix_dst->coef[m13]=matrix_src->coef[m13];
-  matrix_dst->coef[m20]=matrix_src->coef[m20];
-  matrix_dst->coef[m21]=matrix_src->coef[m21];
-  matrix_dst->coef[m22]=matrix_src->coef[m22];
-  matrix_dst->coef[m23]=matrix_src->coef[m23];
-  matrix_dst->coef[m30]=matrix_src->coef[m30];
-  matrix_dst->coef[m31]=matrix_src->coef[m31];
-  matrix_dst->coef[m32]=matrix_src->coef[m32];
-  matrix_dst->coef[m33]=matrix_src->coef[m33];
-}
-
-void matrix3d_zero(TMatrix3D *matrix)
-{
-  matrix->coef[m00]=0.0;
-  matrix->coef[m01]=0.0;
-  matrix->coef[m02]=0.0;
-  matrix->coef[m03]=0.0;
-  matrix->coef[m10]=0.0;
-  matrix->coef[m11]=0.0;
-  matrix->coef[m12]=0.0;
-  matrix->coef[m13]=0.0;
-  matrix->coef[m20]=0.0;
-  matrix->coef[m21]=0.0;
-  matrix->coef[m22]=0.0;
-  matrix->coef[m23]=0.0;
-  matrix->coef[m30]=0.0;
-  matrix->coef[m31]=0.0;
-  matrix->coef[m32]=0.0;
-  matrix->coef[m33]=0.0;
-}
-
-void matrix3d_identity(TMatrix3D *matrix)
-{
-  matrix->coef[m00]=1.0;
-  matrix->coef[m01]=0.0;
-  matrix->coef[m02]=0.0;
-  matrix->coef[m03]=0.0;
-  matrix->coef[m10]=0.0;
-  matrix->coef[m11]=1.0;
-  matrix->coef[m12]=0.0;
-  matrix->coef[m13]=0.0;
-  matrix->coef[m20]=0.0;
-  matrix->coef[m21]=0.0;
-  matrix->coef[m22]=1.0;
-  matrix->coef[m23]=0.0;
-  matrix->coef[m30]=0.0;
-  matrix->coef[m31]=0.0;
-  matrix->coef[m32]=0.0;
-  matrix->coef[m33]=1.0;
-}
-
-uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv)
-{
-  TMatrix3D src, tmp;
-  float det;
-  uint8_t i;
-
-  /* transpose matrix */
-  for (i = 0; i < 4; i++)
-  {
-    src.coef[i] = org->coef[i*4];
-    src.coef[i + 4] = org->coef[i*4 + 1];
-    src.coef[i + 8] = org->coef[i*4 + 2];
-    src.coef[i + 12] = org->coef[i*4 + 3];
-  }
-
-  /* calculate pairs for first 8 elements (cofactors) */
-  tmp.coef[0] = src.coef[10] * src.coef[15];
-  tmp.coef[1] = src.coef[11] * src.coef[14];
-  tmp.coef[2] = src.coef[9] * src.coef[15];
-  tmp.coef[3] = src.coef[11] * src.coef[13];
-  tmp.coef[4] = src.coef[9] * src.coef[14];
-  tmp.coef[5] = src.coef[10] * src.coef[13];
-  tmp.coef[6] = src.coef[8] * src.coef[15];
-  tmp.coef[7] = src.coef[11] * src.coef[12];
-  tmp.coef[8] = src.coef[8] * src.coef[14];
-  tmp.coef[9] = src.coef[10] * src.coef[12];
-  tmp.coef[10] = src.coef[8] * src.coef[13];
-  tmp.coef[11] = src.coef[9] * src.coef[12];
-  /* calculate first 8 elements (cofactors) */
-  inv->coef[0] = (tmp.coef[0]*src.coef[5] + tmp.coef[3]*src.coef[6] + tmp.coef[4]*src.coef[7]) - (tmp.coef[1]*src.coef[5] + tmp.coef[2]*src.coef[6] + tmp.coef[5]*src.coef[7]);
-  inv->coef[1] = (tmp.coef[1]*src.coef[4] + tmp.coef[6]*src.coef[6] + tmp.coef[9]*src.coef[7]) - (tmp.coef[0]*src.coef[4] + tmp.coef[7]*src.coef[6] + tmp.coef[8]*src.coef[7]);
-  inv->coef[2] = (tmp.coef[2]*src.coef[4] + tmp.coef[7]*src.coef[5] + tmp.coef[10]*src.coef[7]) - (tmp.coef[3]*src.coef[4] + tmp.coef[6]*src.coef[5] + tmp.coef[11]*src.coef[7]);
-  inv->coef[3] = (tmp.coef[5]*src.coef[4] + tmp.coef[8]*src.coef[5] + tmp.coef[11]*src.coef[6]) - (tmp.coef[4]*src.coef[4] + tmp.coef[9]*src.coef[5] + tmp.coef[10]*src.coef[6]);
-  inv->coef[4] = (tmp.coef[1]*src.coef[1] + tmp.coef[2]*src.coef[2] + tmp.coef[5]*src.coef[3]) - (tmp.coef[0]*src.coef[1] + tmp.coef[3]*src.coef[2] + tmp.coef[4]*src.coef[3]);
-  inv->coef[5] = (tmp.coef[0]*src.coef[0] + tmp.coef[7]*src.coef[2] + tmp.coef[8]*src.coef[3]) - (tmp.coef[1]*src.coef[0] + tmp.coef[6]*src.coef[2] + tmp.coef[9]*src.coef[3]);
-  inv->coef[6] = (tmp.coef[3]*src.coef[0] + tmp.coef[6]*src.coef[1] + tmp.coef[11]*src.coef[3]) - (tmp.coef[2]*src.coef[0] + tmp.coef[7]*src.coef[1] + tmp.coef[10]*src.coef[3]);
-  inv->coef[7] = (tmp.coef[4]*src.coef[0] + tmp.coef[9]*src.coef[1] + tmp.coef[10]*src.coef[2]) - (tmp.coef[5]*src.coef[0] + tmp.coef[8]*src.coef[1] + tmp.coef[11]*src.coef[2]);
-  /* calculate pairs for second 8 elements (cofactors) */
-  tmp.coef[0] = src.coef[2]*src.coef[7];
-  tmp.coef[1] = src.coef[3]*src.coef[6];
-  tmp.coef[2] = src.coef[1]*src.coef[7];
-  tmp.coef[3] = src.coef[3]*src.coef[5];
-  tmp.coef[4] = src.coef[1]*src.coef[6];
-  tmp.coef[5] = src.coef[2]*src.coef[5];
-  //Streaming SIMD Extensions - Inverse of 4x4 Matrix 8
-  tmp.coef[6] = src.coef[0]*src.coef[7];
-  tmp.coef[7] = src.coef[3]*src.coef[4];
-  tmp.coef[8] = src.coef[0]*src.coef[6];
-  tmp.coef[9] = src.coef[2]*src.coef[4];
-  tmp.coef[10] = src.coef[0]*src.coef[5];
-  tmp.coef[11] = src.coef[1]*src.coef[4];
-  /* calculate second 8 elements (cofactors) */
-  inv->coef[8] = (tmp.coef[0]*src.coef[13] + tmp.coef[3]*src.coef[14] + tmp.coef[4]*src.coef[15]) - (tmp.coef[1]*src.coef[13] + tmp.coef[2]*src.coef[14] + tmp.coef[5]*src.coef[15]);
-  inv->coef[9] = (tmp.coef[1]*src.coef[12] + tmp.coef[6]*src.coef[14] + tmp.coef[9]*src.coef[15]) - (tmp.coef[0]*src.coef[12] + tmp.coef[7]*src.coef[14] + tmp.coef[8]*src.coef[15]);
-  inv->coef[10] = (tmp.coef[2]*src.coef[12] + tmp.coef[7]*src.coef[13] + tmp.coef[10]*src.coef[15]) - (tmp.coef[3]*src.coef[12] + tmp.coef[6]*src.coef[13] + tmp.coef[11]*src.coef[15]);
-  inv->coef[11] = (tmp.coef[5]*src.coef[12] + tmp.coef[8]*src.coef[13] + tmp.coef[11]*src.coef[14]) - (tmp.coef[4]*src.coef[12] + tmp.coef[9]*src.coef[13] + tmp.coef[10]*src.coef[14]);
-  inv->coef[12] = (tmp.coef[2]*src.coef[10] + tmp.coef[5]*src.coef[11] + tmp.coef[1]*src.coef[9]) - (tmp.coef[4]*src.coef[11] + tmp.coef[0]*src.coef[9] + tmp.coef[3]*src.coef[10]);
-  inv->coef[13] = (tmp.coef[8]*src.coef[11] + tmp.coef[0]*src.coef[8] + tmp.coef[7]*src.coef[10]) - (tmp.coef[6]*src.coef[10] + tmp.coef[9]*src.coef[11] + tmp.coef[1]*src.coef[8]);
-  inv->coef[14] = (tmp.coef[6]*src.coef[9] + tmp.coef[11]*src.coef[11] + tmp.coef[3]*src.coef[8]) - (tmp.coef[10]*src.coef[11] + tmp.coef[2]*src.coef[8] + tmp.coef[7]*src.coef[9]);
-  inv->coef[15] = (tmp.coef[10]*src.coef[10] + tmp.coef[4]*src.coef[8] + tmp.coef[9]*src.coef[9]) - (tmp.coef[8]*src.coef[9] + tmp.coef[11]*src.coef[10] + tmp.coef[5]*src.coef[8]);
-  /* calculate determinant */
-  det = src.coef[0]*inv->coef[0] + src.coef[1]*inv->coef[1] + src.coef[2]*inv->coef[2] + src.coef[3]*inv->coef[3];
-  /* calculate matrix inverse */
-  if (det == 0)
-  {
-    det = 0;
-    return 0x00;
-  }
-  else
-  {
-    det = 1 / det;
-  }
-
-  for (i = 0; i < 16; i++)
-    inv->coef[i]*=det;
-
-  return 0x01;
-}
-
-void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector)
-{
-  float Cx = cos(vector->x * 3.141592 / 180.0);
-  float Cy = cos(vector->y * 3.141592 / 180.0);
-  float Cz = cos(vector->z * 3.141592 / 180.0);
-  float Sx = sin(vector->x * 3.141592 / 180.0);
-  float Sy = sin(vector->y * 3.141592 / 180.0);
-  float Sz = sin(vector->z * 3.141592 / 180.0);
-
-  matrix3d_identity(matrix);
-  matrix->coef[0] = Cz * Cy;
-  matrix->coef[1] = Cz * Sy * Sx - Sz * Cx;
-  matrix->coef[2] = Cz * Sy * Cx + Sz * Sx;
-  matrix->coef[3] = point->x;
-  matrix->coef[4] = Sz * Cy;
-  matrix->coef[5] = Sz * Sy * Sx + Cz * Cx;
-  matrix->coef[6] = Sz * Sy * Cx - Cz * Sx;
-  matrix->coef[7] = point->y;
-  matrix->coef[8] = -Sy;
-  matrix->coef[9] = Cy * Sx;
-  matrix->coef[10] = Cy * Cx;
-  matrix->coef[11] = point->z;
-}
-
-void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b)
-{
-  uint8_t i,j,c;
-
-  matrix3d_zero(dst);
-  for(j = 0; j < 4; j++)
-  {
-    for(i = 0; i < 4; i++)
-    {
-      for(c = 0; c < 4; c++)
-      {
-         dst->coef[j*4+i] += a->coef[j*4+c] * b->coef[c*4+i];
-      }
-    }
-  }
-}