118 lines
2.7 KiB
C
118 lines
2.7 KiB
C
#ifndef _BUTIS_MOTOR_MATRIX_H_INCLUDE_
|
|
#define _BUTIS_MOTOR_MATRIX_H_INCLUDE_
|
|
|
|
#include <basic-geometry.h>
|
|
|
|
// =================== Types ==================== //
|
|
|
|
typedef struct {
|
|
float r1c1, r1c2, r1c3, r1c4, r1c5, r1c6;
|
|
float r2c1, r2c2, r2c3, r2c4, r2c5, r2c6;
|
|
float r3c1, r3c2, r3c3, r3c4, r3c5, r3c6;
|
|
float r4c1, r4c2, r4c3, r4c4, r4c5, r4c6;
|
|
float r5c1, r5c2, r5c3, r5c4, r5c5, r5c6;
|
|
float r6c1, r6c2, r6c3, r6c4, r6c5, r6c6;
|
|
} BtMatrixFP32;
|
|
|
|
typedef struct {
|
|
double r1c1, r1c2, r1c3, r1c4, r1c5, r1c6;
|
|
double r2c1, r2c2, r2c3, r2c4, r2c5, r2c6;
|
|
double r3c1, r3c2, r3c3, r3c4, r3c5, r3c6;
|
|
double r4c1, r4c2, r4c3, r4c4, r4c5, r4c6;
|
|
double r5c1, r5c2, r5c3, r5c4, r5c5, r5c6;
|
|
double r6c1, r6c2, r6c3, r6c4, r6c5, r6c6;
|
|
} BtMatrixFP64;
|
|
|
|
// =================== Reset ==================== //
|
|
|
|
inline void bt_matrix_reset_fp32(BtMatrixFP32* matrix)
|
|
{
|
|
matrix->r1c1 = 1.0f;
|
|
matrix->r1c2 = 0.0f;
|
|
matrix->r1c3 = 0.0f;
|
|
matrix->r1c4 = 0.0f;
|
|
matrix->r1c5 = 0.0f;
|
|
matrix->r1c6 = 0.0f;
|
|
|
|
matrix->r2c1 = 0.0f;
|
|
matrix->r2c2 = 1.0f;
|
|
matrix->r2c3 = 0.0f;
|
|
matrix->r2c4 = 0.0f;
|
|
matrix->r2c5 = 0.0f;
|
|
matrix->r2c6 = 0.0f;
|
|
|
|
matrix->r3c1 = 0.0f;
|
|
matrix->r3c2 = 0.0f;
|
|
matrix->r3c3 = 1.0f;
|
|
matrix->r3c4 = 0.0f;
|
|
matrix->r3c5 = 0.0f;
|
|
matrix->r3c6 = 0.0f;
|
|
|
|
matrix->r4c1 = 0.0f;
|
|
matrix->r4c2 = 0.0f;
|
|
matrix->r4c3 = 0.0f;
|
|
matrix->r4c4 = 1.0f;
|
|
matrix->r4c5 = 0.0f;
|
|
matrix->r4c6 = 0.0f;
|
|
|
|
matrix->r5c1 = 0.0f;
|
|
matrix->r5c2 = 0.0f;
|
|
matrix->r5c3 = 0.0f;
|
|
matrix->r5c4 = 0.0f;
|
|
matrix->r5c5 = 1.0f;
|
|
matrix->r5c6 = 0.0f;
|
|
|
|
matrix->r6c1 = 0.0f;
|
|
matrix->r6c2 = 0.0f;
|
|
matrix->r6c3 = 0.0f;
|
|
matrix->r6c4 = 0.0f;
|
|
matrix->r6c5 = 0.0f;
|
|
matrix->r6c6 = 1.0f;
|
|
}
|
|
|
|
inline void bt_matrix_reset_fp64(BtMatrixFP64* matrix)
|
|
{
|
|
matrix->r1c1 = 1.0;
|
|
matrix->r1c2 = 0.0;
|
|
matrix->r1c3 = 0.0;
|
|
matrix->r1c4 = 0.0;
|
|
matrix->r1c5 = 0.0;
|
|
matrix->r1c6 = 0.0;
|
|
|
|
matrix->r2c1 = 0.0;
|
|
matrix->r2c2 = 1.0;
|
|
matrix->r2c3 = 0.0;
|
|
matrix->r2c4 = 0.0;
|
|
matrix->r2c5 = 0.0;
|
|
matrix->r2c6 = 0.0;
|
|
|
|
matrix->r3c1 = 0.0;
|
|
matrix->r3c2 = 0.0;
|
|
matrix->r3c3 = 1.0;
|
|
matrix->r3c4 = 0.0;
|
|
matrix->r3c5 = 0.0;
|
|
matrix->r3c6 = 0.0;
|
|
|
|
matrix->r4c1 = 0.0;
|
|
matrix->r4c2 = 0.0;
|
|
matrix->r4c3 = 0.0;
|
|
matrix->r4c4 = 1.0;
|
|
matrix->r4c5 = 0.0;
|
|
matrix->r4c6 = 0.0;
|
|
|
|
matrix->r5c1 = 0.0;
|
|
matrix->r5c2 = 0.0;
|
|
matrix->r5c3 = 0.0;
|
|
matrix->r5c4 = 0.0;
|
|
matrix->r5c5 = 1.0;
|
|
matrix->r5c6 = 0.0;
|
|
|
|
matrix->r6c1 = 0.0;
|
|
matrix->r6c2 = 0.0;
|
|
matrix->r6c3 = 0.0;
|
|
matrix->r6c4 = 0.0;
|
|
matrix->r6c5 = 0.0;
|
|
matrix->r6c6 = 1.0;
|
|
}
|
|
|
|
#endif
|