Skip to content
Snippets Groups Projects
Commit 74f569a0 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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

Solved some bugs in the EEPROM data structures.
parent 5167a6b4
No related branches found
No related tags found
1 merge request!11Moved the darwin math and kinematics modules to the darwin firmware repo.
Showing with 35 additions and 440 deletions
......@@ -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-
......
#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{
......
#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);
......
......@@ -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};
......
#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);
......
#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
#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
......@@ -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);
......
......@@ -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
......
#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;
}
#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];
}
}
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment