mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 07:53:43 +08:00
Inline math, reused paparazzi math and defines, organize, remove unused
This commit is contained in:
Christophe De Wagter
@@ -4,9 +4,11 @@
|
||||
ifeq ($(ARCH), lpc21)
|
||||
ap.CFLAGS += -DANALOG_IMU -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3 -DUSE_ADC_4 -DUSE_ADC_5 -DUSE_ADC_6 -DUSE_ADC_7
|
||||
|
||||
ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm/dcm.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm/matrix.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm//vector.c
|
||||
ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm/dcm.c
|
||||
ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm/analogimu.c
|
||||
ap.srcs += $(SRC_FIXEDWING)/subsystems/imu/imu_analog.c
|
||||
ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm/analogimu_util.c
|
||||
|
||||
ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm/analogimu.c $(SRC_FIXEDWING)/subsystems/imu/imu_analog.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm/analogimu_util.c
|
||||
endif
|
||||
|
||||
# since there is currently no SITL sim for the Analog IMU, we use the infrared sim
|
||||
|
||||
@@ -26,6 +26,9 @@
|
||||
* \brief Analog IMU Routines
|
||||
*
|
||||
*/
|
||||
|
||||
#include "generated/airframe.h"
|
||||
|
||||
#if ! (defined SITL || defined HITL)
|
||||
|
||||
|
||||
@@ -33,22 +36,24 @@
|
||||
#include "subsystems/imu/imu_analog.h"
|
||||
|
||||
// AHRS attitude computations
|
||||
#include "dcm.h"
|
||||
#include "estimator.h"
|
||||
#include "analogimu_util.h"
|
||||
#include "analogimu.h"
|
||||
|
||||
// Debugging and output
|
||||
#include "led.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
//#include "downlink.h"
|
||||
//#include "ap_downlink.h"
|
||||
#include "estimator.h"
|
||||
#include "sys_time.h"
|
||||
|
||||
#include "dcm.h"
|
||||
#include "analogimu_util.h"
|
||||
#include "analogimu.h"
|
||||
|
||||
#endif
|
||||
|
||||
// variables
|
||||
uint16_t analog_imu_offset[NB_ANALOG_IMU_ADC] = {0,};
|
||||
int adc_average[16] = { 0 };
|
||||
uint16_t analog_imu_offset[NB_ANALOG_IMU_ADC];
|
||||
int adc_average[NB_ANALOG_IMU_ADC];
|
||||
|
||||
// remotely settable
|
||||
float imu_roll_neutral = RadOfDeg(IMU_ROLL_NEUTRAL_DEFAULT);
|
||||
@@ -62,7 +67,7 @@ float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT);
|
||||
*
|
||||
* \return accel[ACC_X], accel[ACC_Y], accel[ACC_Z]
|
||||
*/
|
||||
void accel2ms2( void ) {
|
||||
static void accel2ms2( void ) {
|
||||
accel[ACC_X] = (float)(adc_average[3]) * IMU_ACCEL_X_SENS;
|
||||
accel[ACC_Y] = (float)(adc_average[4]) * IMU_ACCEL_Y_SENS;
|
||||
accel[ACC_Z] = (float)(adc_average[5]) * IMU_ACCEL_Z_SENS;
|
||||
@@ -72,7 +77,7 @@ void accel2ms2( void ) {
|
||||
*
|
||||
* \return gyro[G_ROLL], gyro[G_PITCH], gyro[G_YAW]
|
||||
*/
|
||||
void gyro2rads( void ) {
|
||||
static void gyro2rads( void ) {
|
||||
/** 150 grad/sec 10Bit, 3,3Volt, 1rad = 2Pi/1024 => Pi/512 */
|
||||
gyro[G_ROLL] = (float)(adc_average[0]) * IMU_GYRO_P_SENS;
|
||||
gyro[G_PITCH] = (float)(adc_average[1]) * IMU_GYRO_Q_SENS;
|
||||
@@ -114,9 +119,6 @@ void analog_imu_update( void ) {
|
||||
gyro2rads();
|
||||
}
|
||||
|
||||
/** earth accelecation */
|
||||
volatile float g = 0.;
|
||||
|
||||
// functions
|
||||
|
||||
void analog_imu_downlink( void ) {
|
||||
@@ -130,69 +132,7 @@ void analog_imu_downlink( void ) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Minimalistic version to get angles from acceleration
|
||||
*
|
||||
* \todo why has this function 3 callers ?
|
||||
* \return g, angle[ANG_ROLL], angle[ANG_PITCH]
|
||||
*/
|
||||
void accel2euler( void ) {
|
||||
// Calculate g ( ||g_vec|| )
|
||||
g = sqrt(accel[ACC_X] * accel[ACC_X] +
|
||||
accel[ACC_Y] * accel[ACC_Y] +
|
||||
accel[ACC_Z] * accel[ACC_Z]);
|
||||
if( g < 0.01 && g > -0.01 )
|
||||
{
|
||||
g=0.01;
|
||||
}else{
|
||||
//nothing
|
||||
}
|
||||
//values in radians
|
||||
#define NEW
|
||||
#ifdef OLD
|
||||
angle[ANG_PITCH] = -asinf( accel[ACC_X] / g );
|
||||
angle[ANG_ROLL] = asinf( accel[ACC_Y] / g );
|
||||
angle[ANG_YAW] = 0.0;
|
||||
#endif
|
||||
|
||||
#ifdef NEW
|
||||
|
||||
float a1 = accel[ACC_X] / -g;
|
||||
|
||||
if(a1 > 1.0 && a1 >= 0.0){
|
||||
a1 = 1.0;
|
||||
} else if(a1 < -1.0 && a1 < 0.0){
|
||||
a1 = -1.0;
|
||||
}
|
||||
|
||||
angle[ANG_PITCH] = asinf( a1 );
|
||||
|
||||
if(accel[ACC_Z] < 0 && angle[ANG_PITCH] > 0) angle[ANG_PITCH] = + 3.145/2 + (3.145/2 - angle[ANG_PITCH]);
|
||||
else if (accel[ACC_Z] < 0 && angle[ANG_PITCH] < 0) angle[ANG_PITCH] = -3.145/2 - (3.145/2 + angle[ANG_PITCH]);
|
||||
|
||||
|
||||
if( accel[ACC_Z] < 0.01 && accel[ACC_Z] > -0.01 )
|
||||
{
|
||||
accel[ACC_Z]=0.01;
|
||||
}else{
|
||||
//nothing
|
||||
}
|
||||
|
||||
|
||||
angle[ANG_ROLL] = atan2f( accel[ACC_Y], accel[ACC_Z] );
|
||||
//angle[ANG_PITCH] = -atan2f( accel[ACC_X] , accel[ACC_Z] );
|
||||
angle[ANG_YAW] = 0.0;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
void estimator_update_state_analog_imu( void ) {
|
||||
#undef ANGLE_FROM_ACCEL
|
||||
#ifdef ANGLE_FROM_ACCEL
|
||||
estimator_phi = (float)(atan2f((float)((analog_raw[6]-510)),(float)(-(analog_raw[7]-510))));
|
||||
estimator_theta = (float)(atan2f((float)(-(analog_raw[5]-510)),(float)(-(analog_raw[7]-510))));
|
||||
#else
|
||||
|
||||
analog_imu_update();
|
||||
|
||||
@@ -208,6 +148,8 @@ void estimator_update_state_analog_imu( void ) {
|
||||
|
||||
Matrix_update();
|
||||
Normalize();
|
||||
|
||||
|
||||
Drift_correction();
|
||||
Euler_angles();
|
||||
|
||||
@@ -218,6 +160,5 @@ void estimator_update_state_analog_imu( void ) {
|
||||
//estimator_theta = angle[ANG_PITCH];
|
||||
estimator_psi = euler[EULER_YAW];
|
||||
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -32,8 +32,6 @@
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#include "generated/airframe.h"
|
||||
|
||||
extern float imu_roll_neutral;
|
||||
extern float imu_pitch_neutral;
|
||||
|
||||
@@ -42,10 +40,7 @@ void analog_imu_init( void );
|
||||
void analog_imu_update( void );
|
||||
void analog_imu_downlink( void );
|
||||
void analogconversion( void );
|
||||
void accel2euler( void );
|
||||
void accel2ms2( void );
|
||||
void estimator_update_state_analog_imu( void );
|
||||
void gyro2rads( void );
|
||||
void analog_imu_offset_set( void );
|
||||
|
||||
#endif // _ANALOGIMU_H_
|
||||
|
||||
@@ -1,48 +1,98 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "vector.h"
|
||||
#include "matrix.h"
|
||||
|
||||
#ifdef ANALOG_IMU
|
||||
#include "analogimu.h"
|
||||
#include "analogimu_util.h"
|
||||
#endif // ANALOG_IMU
|
||||
#include "std.h"
|
||||
|
||||
#include "dcm.h"
|
||||
|
||||
// Own Math
|
||||
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
|
||||
#define abs(x) ((x)>0?(x):-(x))
|
||||
|
||||
// DCM Working variables
|
||||
float Gyro_Vector[3] = {0,0,0};
|
||||
float Accel_Vector[3] = {0,0,0};
|
||||
|
||||
// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
|
||||
// Positive pitch : nose up
|
||||
// Positive roll : right wing down
|
||||
// Positive yaw : clockwise
|
||||
|
||||
|
||||
// DCM Working variables
|
||||
float G_Dt=0.05;
|
||||
|
||||
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
|
||||
float Omega_P[3]= {0,0,0};//Omega Proportional correction
|
||||
float Omega_I[3]= {0,0,0};//Omega Integrator
|
||||
float Omega[3]= {0,0,0};
|
||||
float Gyro_Vector[3] = {0,0,0};
|
||||
float Accel_Vector[3] = {0,0,0};
|
||||
|
||||
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
|
||||
float Omega_P[3]= {0,0,0}; //Omega Proportional correction
|
||||
float Omega_I[3]= {0,0,0}; //Omega Integrator
|
||||
float Omega[3]= {0,0,0};
|
||||
|
||||
float DCM_Matrix[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
|
||||
float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
|
||||
float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Estimated angles as Euler
|
||||
*/
|
||||
float euler[EULER_LAST] = {0.};
|
||||
|
||||
/**************************************************/
|
||||
|
||||
// Algebra
|
||||
|
||||
//Computes the dot product of two vectors
|
||||
static inline float Vector_Dot_Product(float vector1[3],float vector2[3])
|
||||
{
|
||||
return vector1[0]*vector2[0] + vector1[1]*vector2[1] + vector1[2]*vector2[2];
|
||||
}
|
||||
|
||||
//Computes the cross product of two vectors
|
||||
static inline void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
|
||||
{
|
||||
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
|
||||
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
|
||||
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
|
||||
}
|
||||
|
||||
//Multiply the vector by a scalar.
|
||||
static inline void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
|
||||
{
|
||||
vectorOut[0]=vectorIn[0]*scale2;
|
||||
vectorOut[1]=vectorIn[1]*scale2;
|
||||
vectorOut[2]=vectorIn[2]*scale2;
|
||||
}
|
||||
|
||||
static inline void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
|
||||
{
|
||||
vectorOut[0]=vectorIn1[0]+vectorIn2[0];
|
||||
vectorOut[1]=vectorIn1[1]+vectorIn2[1];
|
||||
vectorOut[2]=vectorIn1[2]+vectorIn2[2];
|
||||
}
|
||||
|
||||
/*
|
||||
#define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) { \
|
||||
_m_a2c[0] = (_m_b2c[0]*_m_a2b[0] + _m_b2c[1]*_m_a2b[3] + _m_b2c[2]*_m_a2b[6]); \
|
||||
_m_a2c[1] = (_m_b2c[0]*_m_a2b[1] + _m_b2c[1]*_m_a2b[4] + _m_b2c[2]*_m_a2b[7]); \
|
||||
_m_a2c[2] = (_m_b2c[0]*_m_a2b[2] + _m_b2c[1]*_m_a2b[5] + _m_b2c[2]*_m_a2b[8]); \
|
||||
_m_a2c[3] = (_m_b2c[3]*_m_a2b[0] + _m_b2c[4]*_m_a2b[3] + _m_b2c[5]*_m_a2b[6]); \
|
||||
_m_a2c[4] = (_m_b2c[3]*_m_a2b[1] + _m_b2c[4]*_m_a2b[4] + _m_b2c[5]*_m_a2b[7]); \
|
||||
_m_a2c[5] = (_m_b2c[3]*_m_a2b[2] + _m_b2c[4]*_m_a2b[5] + _m_b2c[5]*_m_a2b[8]); \
|
||||
_m_a2c[6] = (_m_b2c[6]*_m_a2b[0] + _m_b2c[7]*_m_a2b[3] + _m_b2c[8]*_m_a2b[6]); \
|
||||
_m_a2c[7] = (_m_b2c[6]*_m_a2b[1] + _m_b2c[7]*_m_a2b[4] + _m_b2c[8]*_m_a2b[7]); \
|
||||
_m_a2c[8] = (_m_b2c[6]*_m_a2b[2] + _m_b2c[7]*_m_a2b[5] + _m_b2c[8]*_m_a2b[8]); \
|
||||
}
|
||||
*/
|
||||
|
||||
static inline void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
|
||||
{
|
||||
float op[3];
|
||||
for(int x=0; x<3; x++)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
for(int w=0; w<3; w++)
|
||||
{
|
||||
op[w]=a[x][w]*b[w][y];
|
||||
}
|
||||
mat[x][y]=op[0]+op[1]+op[2];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void Normalize(void)
|
||||
{
|
||||
float error=0;
|
||||
@@ -218,12 +268,12 @@ void Drift_correction(void)
|
||||
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
|
||||
// Dynamic weighting of accelerometer info (reliability filter)
|
||||
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
|
||||
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
|
||||
Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
|
||||
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
|
||||
imu_health += tempfloat;
|
||||
imu_health = constrain(imu_health,129,65405);
|
||||
Bound(imu_health,129,65405);
|
||||
#endif
|
||||
|
||||
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
|
||||
@@ -280,21 +330,18 @@ void Drift_correction(void)
|
||||
}
|
||||
/**************************************************/
|
||||
|
||||
void Accel_adjust(void)
|
||||
{
|
||||
Accel_Vector[1] += speed_3d*Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
|
||||
Accel_Vector[2] -= speed_3d*Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
|
||||
}
|
||||
/**************************************************/
|
||||
|
||||
void Matrix_update(void)
|
||||
{
|
||||
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
|
||||
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
|
||||
|
||||
if (gps_mode==3) Accel_adjust(); //Remove centrifugal acceleration.
|
||||
if (gps_mode==3) //Remove centrifugal acceleration.
|
||||
{
|
||||
Accel_Vector[1] += speed_3d*Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
|
||||
Accel_Vector[2] -= speed_3d*Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
|
||||
}
|
||||
|
||||
|
||||
#define OUTPUTMODE 1
|
||||
#if OUTPUTMODE==1 // With corrected data (drift correction)
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
|
||||
@@ -330,25 +377,16 @@ void Matrix_update(void)
|
||||
|
||||
void Euler_angles(void)
|
||||
{
|
||||
//#define OUTPUTMODE 2
|
||||
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
|
||||
euler[EULER_ROLL] = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
|
||||
//euler[EULER_PITCH] = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
|
||||
//todo: chni:ordentlich lösen!
|
||||
euler[EULER_PITCH] = -asin((Accel_Vector[0])/9.81); // asin(acc_x)
|
||||
euler[EULER_PITCH] = -asin((Accel_Vector[0])/GRAVITY); // asin(acc_x)
|
||||
euler[EULER_YAW] = 0;
|
||||
#else
|
||||
//pitch = -asin(DCM_Matrix[2][0]);
|
||||
//roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
|
||||
//yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
|
||||
#ifndef ANALOGIMU_ROTATED
|
||||
euler[EULER_PITCH] = -asin(DCM_Matrix[2][0]);
|
||||
euler[EULER_ROLL] = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
|
||||
#else
|
||||
euler[EULER_ROLL] = -asin(DCM_Matrix[2][0]);
|
||||
euler[EULER_PITCH] = -atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
|
||||
#endif
|
||||
|
||||
euler[EULER_PITCH] = -asin(DCM_Matrix[2][0]);
|
||||
euler[EULER_ROLL] = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
|
||||
euler[EULER_YAW] = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
|
||||
euler[EULER_YAW] += M_PI; // Rotating the angle 180deg to fit for PPRZ
|
||||
#endif
|
||||
|
||||
@@ -1,8 +1,3 @@
|
||||
void Euler_angles(void);
|
||||
void Accel_adjust(void);
|
||||
void Drift_correction(void);
|
||||
void Normalize(void);
|
||||
|
||||
|
||||
// Inputs for DCM
|
||||
extern float Gyro_Vector[3];
|
||||
@@ -10,7 +5,17 @@ extern float Accel_Vector[3];
|
||||
|
||||
// Integrate Inertial
|
||||
void Matrix_update(void);
|
||||
void Normalize(void);
|
||||
|
||||
// Input GPS/Pressure/Magnetometer data
|
||||
|
||||
// Correct
|
||||
void Drift_correction(void);
|
||||
|
||||
// Get outputs
|
||||
void Euler_angles(void);
|
||||
enum euler_idx_t { EULER_ROLL, EULER_PITCH, EULER_YAW, EULER_LAST };
|
||||
extern float euler[3];
|
||||
|
||||
// DCM Parameters
|
||||
|
||||
@@ -20,14 +25,13 @@ void Matrix_update(void);
|
||||
#define Kp_YAW 1.2 //High yaw drift correction gain - use with caution!
|
||||
#define Ki_YAW 0.00005
|
||||
|
||||
|
||||
|
||||
/** defines for euler vector */
|
||||
enum euler_idx_t { EULER_ROLL, EULER_PITCH, EULER_YAW, EULER_LAST };
|
||||
|
||||
#define M_PI 3.14159265358979323846
|
||||
#define GRAVITY 9.81
|
||||
|
||||
|
||||
/** output vector with angles in rad */
|
||||
extern float euler[EULER_LAST];
|
||||
#define OUTPUTMODE 1
|
||||
// Mode 0 = DCM integration without Ki gyro bias
|
||||
// Mode 1 = DCM integration with Kp and Ki
|
||||
// Mode 2 = direct accelerometer -> euler
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,24 +0,0 @@
|
||||
/**************************************************/
|
||||
//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
|
||||
|
||||
#include "matrix.h"
|
||||
|
||||
void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
|
||||
{
|
||||
float op[3];
|
||||
for(int x=0; x<3; x++)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
for(int w=0; w<3; w++)
|
||||
{
|
||||
op[w]=a[x][w]*b[w][y];
|
||||
}
|
||||
mat[x][y]=0;
|
||||
mat[x][y]=op[0]+op[1]+op[2];
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -1,2 +0,0 @@
|
||||
|
||||
void Matrix_Multiply(float a[3][3],float b[3][3],float mat[3][3]);
|
||||
@@ -1,42 +0,0 @@
|
||||
#include "vector.h"
|
||||
|
||||
//Computes the dot product of two vectors
|
||||
float Vector_Dot_Product(float vector1[3],float vector2[3])
|
||||
{
|
||||
float op=0;
|
||||
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
op+=vector1[c]*vector2[c];
|
||||
}
|
||||
|
||||
return op;
|
||||
}
|
||||
|
||||
//Computes the cross product of two vectors
|
||||
void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
|
||||
{
|
||||
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
|
||||
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
|
||||
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
|
||||
}
|
||||
|
||||
//Multiply the vector by a scalar.
|
||||
void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
|
||||
{
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
vectorOut[c]=vectorIn[c]*scale2;
|
||||
}
|
||||
}
|
||||
|
||||
void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
|
||||
{
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
vectorOut[c]=vectorIn1[c]+vectorIn2[c];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,5 +0,0 @@
|
||||
|
||||
void Vector_Add(float vectorOut[3],float vectorIn1[3],float vectorIn2[3]);
|
||||
void Vector_Scale(float vectorOut[3],float vectorIn[3],float scale2);
|
||||
void Vector_Cross_Product(float vectorOut[3],float v1[3],float v2[3]);
|
||||
float Vector_Dot_Product(float vector1[3],float vector2[3]);
|
||||
Reference in New Issue
Block a user