Commit 9fe788e8 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added the walking algorithm to the motion manager.

parent 62d5566f
......@@ -86,7 +86,7 @@ $(BUILD_PATH)/%.o: $(USART_PATH)/src/%.c
$(CC) -c $(CFLAGS) -o $@ $<
$(MAIN_OUT_ELF): mkdir_build $(BIOLOID_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o)
$(LD) $(LDFLAGS) $(BIOLOID_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) --output $@
$(LD) $(LDFLAGS) $(BIOLOID_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) -lm --output $@
$(MAIN_OUT_HEX): $(MAIN_OUT_ELF)
$(OBJCP) $(OBJCPFLAGS_HEX) $< $@
......
......@@ -8,9 +8,6 @@ extern "C" {
#include "stm32f4xx.h"
#include "motion_pages.h"
extern int64_t action_current_angles[PAGE_MAX_NUM_SERVOS];
extern uint8_t action_slope[PAGE_MAX_NUM_SERVOS];
// public functions
void action_init(uint16_t period_us);
inline void action_set_period(uint16_t period_us);
......
#ifndef _BIOLOID_KINEMATICS_H
#define _BIOLOID_KINEMATICS_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f4xx.h"
#define BIOLOID_LEG_SIDE_OFFSET 38.5 //mm
#define BIOLOID_PELVIS_LENGTH 29.0 //mm
#define BIOLOID_THIGH_LENGTH 76.388 //mm
#define BIOLOID_CALF_LENGTH 76.388 //mm
#define BIOLOID_KNEE_DEPTH 14.5 //mm
#define BIOLOID_ANKLE_LENGTH 33.68 //mm
#define BIOLOID_LEG_LENGTH (BIOLOID_THIGH_LENGTH + BIOLOID_CALF_LENGTH + BIOLOID_ANKLE_LENGTH) //mm
// public functions
uint8_t bioloid_leg_ik(float *out, float x, float y, float z, float a, float b, float c);
#ifdef __cplusplus
}
#endif
#endif
#ifndef _BIOLOID_MATH_H
#define _BIOLOID_MATH_H
#include "stm32f4xx.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);
#endif
......@@ -50,6 +50,23 @@ extern "C" {
#define MM_SERVO31_OFFSET ((unsigned short int)0x0030)
#define GYRO_FB_ADC_CH_OFFSET ((unsigned short int)0x0031)
#define GYRO_LR_ADC_CH_OFFSET ((unsigned short int)0x0032)
#define WALK_X_OFFSET ((unsigned short int)0x0033)
#define WALK_Y_OFFSET ((unsigned short int)0x0034)
#define WALK_Z_OFFSET ((unsigned short int)0x0035)
#define WALK_ROLL_OFFSET ((unsigned short int)0x0036)
#define WALK_PITCH_OFFSET ((unsigned short int)0x0037)
#define WALK_YAW_OFFSET ((unsigned short int)0x0038)
#define WALK_HIP_PITCH_OFF ((unsigned short int)0x0039)
#define WALK_PERIOD_TIME ((unsigned short int)0x003B)
#define WALK_DSP_RATIO ((unsigned short int)0x003D)
#define WALK_STEP_FW_BW_RATIO ((unsigned short int)0x003E)
#define WALK_FOOT_HEIGHT ((unsigned short int)0x003F)
#define WALK_SWING_RIGHT_LEFT ((unsigned short int)0x0040)
#define WALK_SWING_TOP_DOWN ((unsigned short int)0x0041)
#define WALK_PELVIS_OFFSET ((unsigned short int)0x0042)
#define WALK_ARM_SWING_GAIN ((unsigned short int)0x0043)
#define WALK_MAX_VEL ((unsigned short int)0x0044)
#define WALK_MAX_ROT_VEL ((unsigned short int)0x0045)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
......@@ -105,6 +122,25 @@ typedef enum {
BIOLOID_MM_SERVO31_OFFSET = MM_SERVO31_OFFSET, // angle offset in fixed point format 1-3|4
BIOLIOD_GYRO_FB_ADC_CH = GYRO_FB_ADC_CH_OFFSET,
BIOLIOD_GYRO_LR_ADC_CH = GYRO_LR_ADC_CH_OFFSET,
BIOLOID_WALK_X_OFFSET = WALK_X_OFFSET,
BIOLOID_WALK_Y_OFFSET = WALK_Y_OFFSET,
BIOLOID_WALK_Z_OFFSET = WALK_Z_OFFSET,
BIOLOID_WALK_ROLL_OFFSET = WALK_ROLL_OFFSET,
BIOLOID_WALK_PITCH_OFFSET = WALK_PITCH_OFFSET,
BIOLOID_WALK_YAW_OFFSET = WALK_YAW_OFFSET,
BIOLOID_WALK_HIP_PITCH_OFF_L = WALK_HIP_PITCH_OFF,
BIOLOID_WALK_HIP_PITCH_OFF_H = WALK_HIP_PITCH_OFF+1,
BIOLOID_WALK_PERIOD_TIME_L = WALK_PERIOD_TIME,
BIOLOID_WALK_PERIOD_TIME_H = WALK_PERIOD_TIME+1,
BIOLOID_WALK_DSP_RATIO = WALK_DSP_RATIO,
BIOLOID_WALK_STEP_FW_BW_RATIO = WALK_STEP_FW_BW_RATIO,
BIOLOID_WALK_FOOT_HEIGHT = WALK_FOOT_HEIGHT,
BIOLOID_WALK_SWING_RIGHT_LEFT = WALK_SWING_RIGHT_LEFT,
BIOLOID_WALK_SWING_TOP_DOWN = WALK_SWING_TOP_DOWN,
BIOLOID_WALK_PELVIS_OFFSET = WALK_PELVIS_OFFSET,
BIOLOID_WALK_ARM_SWING_GAIN = WALK_ARM_SWING_GAIN,
BIOLOID_WALK_MAX_VEL = WALK_MAX_VEL,
BIOLOID_WALK_MAX_ROT_VEL = WALK_MAX_ROT_VEL,
BIOLOID_USER1_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | blink | toggle | value | internally used
BIOLOID_USER1_LED_PERIOD_L = 0x0101,
......@@ -268,7 +304,12 @@ typedef enum {
BIOLOID_GYRO_FWD_FALL_THD = 0x017E,
BIOLOID_GYRO_BWD_FALL_THD = 0x017F,
BIOLOID_GYRO_LEFT_FALL_THD = 0x0180,
BOILOID_GYRO_RIGHT_FALL_THD = 0x0181
BOILOID_GYRO_RIGHT_FALL_THD = 0x0181,
BIOLOID_WALK_CNTRL = 0x0182, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// current phase walking stop walking start walking
BIOLOID_WALK_STEP_FW_BW = 0x0183,
BIOLOID_WALK_STEP_LEFT_RIGHT = 0x0184,
BIOLOID_WALK_STEP_DIRECTION = 0x0185
} bioloid_registers;
#define GPIO_BASE_ADDRESS 0x0100
......@@ -291,7 +332,10 @@ typedef enum {
#define MANAGER_BASE_ADDRESS 0x0128
#define MANAGER_MEM_LENGTH 82
#define MANAGER_EEPROM_LENGTH 10
#define MANAGER_EEPROM_BASE1 0x0006
#define MANAGER_EEPROM_LENGTH1 10
#define MANAGER_EEPROM_BASE2 0x0011
#define MANAGER_EEPROM_LENGTH2 32
#define MANAGER_ENABLE 0x01
#define MANAGER_EN_BAL 0x02
#define MANAGER_EN_PWR 0x04
......@@ -315,6 +359,15 @@ typedef enum {
#define GYRO_EN_FALL_DET 0x04
#define GYRO_EN_FALL_DET_INT 0x08
#define WALK_BASE_ADDRESS 0x0182
#define WALK_MEM_LENGTH 4
#define WALK_EEPROM_ADDRESS 0x0033
#define WALK_EEPROM_LENGTH 19
#define WALK_START 0x01
#define WALK_STOP 0x02
#define WALK_STATUS 0x10
#define WALK_PHASE 0xC0
#ifdef __cplusplus
}
#endif
......
......@@ -83,7 +83,7 @@ extern "C" {
#define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */
#define NB_OF_VAR ((uint8_t)0x33)
#define NB_OF_VAR ((uint8_t)0x46)
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
......
#ifndef _WALKING_H
#define _WALKING_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f4xx.h"
#include "motion_manager.h"
// public functions
void walking_init(uint16_t period_us);
inline void walking_set_period(uint16_t period_us);
inline uint16_t walking_get_period(void);
void walking_start(void);
void walking_stop(void);
uint8_t is_walking(void);
// operation functions
uint8_t walking_in_range(unsigned short int address, unsigned short int length);
void walking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
void walking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
// motion manager interface functions
void walking_process(void);
#ifdef __cplusplus
}
#endif
#endif
......@@ -53,23 +53,23 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
if(ram_in_range(BIOLOID_ID,address,length))
{
dyn_slave_set_address(&bioloid_dyn_slave,data[BIOLOID_ID-address]);
ram_write_byte(BIOLOID_ID,data[BIOLOID_ID-address]);
ram_data[BIOLOID_ID]=data[BIOLOID_ID-address];
}
if(ram_in_range(BIOLOID_BAUD_RATE,address,length))
{
bioloid_comm_init.BaudRate=2000000/(data[BIOLOID_BAUD_RATE-address]+1);
usart3_config(&bioloid_dyn_slave_comm,&bioloid_comm_init);
ram_write_byte(BIOLOID_BAUD_RATE,data[BIOLOID_BAUD_RATE-address]);
ram_data[BIOLOID_BAUD_RATE]=data[BIOLOID_BAUD_RATE-address];
}
if(ram_in_range(BIOLOID_RETURN_DELAY_TIME,address,length))
{
dyn_slave_set_return_delay(&bioloid_dyn_slave,data[BIOLOID_RETURN_DELAY_TIME-address]);
ram_write_byte(BIOLOID_RETURN_DELAY_TIME,data[BIOLOID_RETURN_DELAY_TIME-address]);
ram_data[BIOLOID_RETURN_DELAY_TIME]=data[BIOLOID_RETURN_DELAY_TIME-address];
}
if(ram_in_range(BIOLOID_RETURN_LEVEL,address,length))
{
dyn_slave_set_return_level(&bioloid_dyn_slave,data[BIOLOID_RETURN_LEVEL-address]);
ram_write_byte(BIOLOID_RETURN_LEVEL,data[BIOLOID_RETURN_LEVEL-address]);
ram_data[BIOLOID_RETURN_LEVEL]=data[BIOLOID_RETURN_LEVEL-address];
}
// GPIO commands
if(gpio_in_range(address,length))
......@@ -98,7 +98,6 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
void bioloid_on_ping(void)
{
gpio_toggle_led(RXD_LED);
}
/* interrupt service routines */
......
......@@ -72,6 +72,10 @@ void gyro_process_read_cmd(unsigned short int address,unsigned short int length,
void gyro_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
// gyro channels
if(ram_in_range(BIOLIOD_GYRO_FB_ADC_CH,address,length))
ram_data[BIOLIOD_GYRO_FB_ADC_CH]=data[BIOLIOD_GYRO_FB_ADC_CH-address];
if(ram_in_range(BIOLIOD_GYRO_LR_ADC_CH,address,length))
ram_data[BIOLIOD_GYRO_LR_ADC_CH]=data[BIOLIOD_GYRO_LR_ADC_CH-address];
}
#include "bioloid_kinematics.h"
#include "bioloid_math.h"
#include <math.h>
uint8_t bioloid_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 - BIOLOID_LEG_LENGTH);
matrix3d_set_transform(&Tad,&position,&orientation);
vec.x = x + Tad.coef[2] * BIOLOID_ANKLE_LENGTH;
vec.y = y + Tad.coef[6] * BIOLOID_ANKLE_LENGTH;
vec.z = (z - BIOLOID_LEG_LENGTH) + Tad.coef[10] * BIOLOID_ANKLE_LENGTH;
// Get Knee
_Rac = vector3d_length(&vec);
_Acos = acos((_Rac * _Rac - BIOLOID_THIGH_LENGTH * BIOLOID_THIGH_LENGTH - BIOLOID_CALF_LENGTH * BIOLOID_CALF_LENGTH) / (2 * BIOLOID_THIGH_LENGTH * BIOLOID_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] - BIOLOID_ANKLE_LENGTH) * (Tda.coef[11] - BIOLOID_ANKLE_LENGTH));
_m = (_k * _k - _l * _l - BIOLOID_ANKLE_LENGTH * BIOLOID_ANKLE_LENGTH) / (2 * _l * BIOLOID_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, -BIOLOID_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)) * BIOLOID_CALF_LENGTH;
_l = -BIOLOID_THIGH_LENGTH - cos(*(out + 3)) * BIOLOID_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 "bioloid_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 * PI / 180.0);
float Cy = cos(vector->y * PI / 180.0);
float Cz = cos(vector->z * PI / 180.0);
float Sx = sin(vector->x * PI / 180.0);