Commit 2b0925c0 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

no progress

parent 1610d454
File added
No preview for this file type
......@@ -36,13 +36,13 @@
#define USE_SUPERVISOR 0
#define FLOCK_SIZE 5
#define NEIGHBOR_BOUND
#define ROBOT_TO_MONITOR 0
#define TIME_STEP 64 // [ms] Length of time step
#define EPS 0.08
#define MIGR_SPEED 0.055
#define SPEED_CORR 0.02
#define ROBOT_TO_MONITOR 2
#define TIME_STEP 64 // [ms] Length of time step
#define EPS 0.03
#define MIGR_SPEED 0.06
#define SPEED_CORR 0.05
#define MAX_ACC 0.05
// ********
#define NB_SENSORS 8 // Number of distance sensors
......@@ -56,8 +56,8 @@ static void reset(){
e_led_clear();
#else
wb_robot_init();
/*if(utilities_getRobotId() == ROBOT_TO_MONITOR)
utilities_changeDebugState(1);*/
if(utilities_getRobotId() == ROBOT_TO_MONITOR)
utilities_changeDebugState(1);
printf("Reset: robot %d\n",utilities_getRobotId());
#endif
......@@ -70,26 +70,30 @@ static void reset(){
void migratory(float* my_speed, float* my_position){
// realign migration
//if (my_position[Y] > EPS) my_speed[Y] -= SPEED_CORR;
//if (my_position[Y] < -EPS) my_speed[Y] += SPEED_CORR;
void migratory(double* my_speed, double* my_position, double laplacianWeight, double frontSpeedRatio){
//realign migration
if (my_position[Y] > EPS) my_speed[Y] -= SPEED_CORR;
if (my_position[Y] < -EPS) my_speed[Y] += SPEED_CORR;
my_speed[X] = my_speed[X] + MIGR_SPEED /* * (1- laplacianWeight)*/;
// add constant migration to the other side
my_speed[X] += MIGR_SPEED;
}
int main(){
reset(); // Resetting the robot
double my_position[3] = {0,0,0};
float my_speed[2] = {0,0};
float weights[2] = {1,-20};
double my_speed[2] = {0,0};
double old_speed[2] = {0,0};
double weights[2] = {1,-20};
int applied_speed[2] = {0,0};
double velocityRatio = 1;
Measurement* list;
for(;;){
numberPrint++;
int obstacle = 0;
if(utilities_debug()) printf("___________________\n");
if(utilities_debug()) printf("Robot %d\n",utilities_getRobotId());
......@@ -103,26 +107,43 @@ int main(){
list = rangeAndBearing_getRelativeRobotPositions();
}
list = infrared_updateList(list, &obstacle);
list = infrared_updateList(list, &velocityRatio);
deadReckoning_updateSelfPosition(my_position, applied_speed);
float my_speed_tmp[2];
double my_speed_tmp[2];
// compute new speed according to laplace method in initial referential
laplace_compute(list, my_speed, weights, my_position);
if(utilities_debug()) printf("Before migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
double laplacianWeight = (0.4-exp(-sqrtf(my_speed[X]*my_speed[X]+my_speed[Y]*my_speed[Y])*20)*0.4);
if(utilities_debug()) printf("Before migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
utilities_transformReferential(my_speed, my_speed_tmp, -my_position[2]); //from Rt to R0
// add migration urge and realignment in intial referential
if(utilities_debug()) printf("Velocity ratio = %f / Laplacian Weight: %f\n", velocityRatio, laplacianWeight);
migratory(my_speed_tmp, my_position, laplacianWeight, velocityRatio);
migratory(my_speed_tmp, my_position);
// change speed in inital referential to current referential
utilities_transformReferential(my_speed_tmp, my_speed, my_position[2]); //from R0 to Rt
if(utilities_debug()) printf("After migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
//my_speed[X] = velocityRatio * my_speed[X] + (1-velocityRatio) * old_speed[X];
if(my_speed[X] - old_speed[X] > MAX_ACC) my_speed[X] = old_speed[X] + MAX_ACC;
if(my_speed[X] - old_speed[X] < -MAX_ACC) my_speed[X] = old_speed[X] - MAX_ACC;
if(my_speed[Y] - old_speed[Y] > MAX_ACC) my_speed[Y] = old_speed[Y] + MAX_ACC;
if(my_speed[Y] - old_speed[Y] < -MAX_ACC) my_speed[Y] = old_speed[Y] - MAX_ACC;
old_speed[X] = my_speed[X];
old_speed[Y] = my_speed[Y];
if(velocityRatio < 1.) velocityRatio += 0.05;
if(utilities_debug()) printf("After readapt x = %f, y = %f\n", my_speed[X], my_speed[Y]);
// if(utilities_getRobotId() == 8) printf("%f,%f\n", my_speed[X], my_speed[Y]);
//if(utilities_getRobotId() == 8) printf("time step: %d\n", numberPrint);
// apply speed to motors
motors_applySpeed(my_speed, applied_speed);
......
......@@ -33,14 +33,14 @@ void deadReckoning_init(){}
void deadReckoning_updateSelfPosition(double* my_position, int* applied_speed){
// Compute deltas of the robot
float dr = (float) applied_speed[RIGHT] * SPEED_UNIT_RADS * WHEEL_RADIUS * DELTA_T;
float dl = (float) applied_speed[LEFT] * SPEED_UNIT_RADS * WHEEL_RADIUS * DELTA_T;
float du = (dr + dl)/2.0;
float dtheta = (dr - dl)/AXLE_LENGTH;
double dr = (double) applied_speed[RIGHT] * SPEED_UNIT_RADS * WHEEL_RADIUS * DELTA_T;
double dl = (double) applied_speed[LEFT] * SPEED_UNIT_RADS * WHEEL_RADIUS * DELTA_T;
double du = (dr + dl)/2.0;
double dtheta = (dr - dl)/AXLE_LENGTH;
// Compute deltas in the environment
float dx = du * cosf(my_position[2]);
float dy = du * sinf(my_position[2]);
double dx = du * cosf(my_position[2]);
double dy = du * sinf(my_position[2]);
// Update position ??? maybe add time step???
......
......@@ -4,6 +4,6 @@
#include "measurements.h"
void infrared_init();
Measurement* infrared_updateList(Measurement* measurementList, int* obstacle);
Measurement* infrared_updateList(Measurement* measurementList, double* frontSpeedRatio);
#endif
......@@ -11,7 +11,7 @@
* Returns:
* --
*/
void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* weights, float* my_position);
void laplace_compute(Measurement* objectsInSurround, double* newSpeed, double* weights, double* my_position);
void laplace_init();
#endif
......@@ -11,9 +11,9 @@ typedef struct Measurement_t Measurement;
struct Measurement_t{
int robotID; // -1 if not a robot
float relativePosition[2];
float biasPosition[2]; // position in the graph of the robot
float weight;
double relativePosition[2];
double biasPosition[2]; // position in the graph of the robot
double weight;
Measurement* next;
};
......
......@@ -5,6 +5,6 @@
void motors_init();
void motors_applySpeed(float* speed, int* appliedSpeed);
void motors_applySpeed(double* speed, int* appliedSpeed);
#endif
......@@ -7,9 +7,56 @@ extern int numberPrint;
#define LEFT 0
#define RIGHT 1
void utilities_changeDebugState(int enableDebugTemp);
// Debug functions
void utilities_changeDebugState(int enabledDebugTemp);
int utilities_debug();
void utilities_transformReferential(float* speed, float* new_speed, float theta);
// Matrix operations fonctions
void utilities_transformReferential(double* initialReferencial, double* newReferencial, double theta);
void utilities_transformReferentialImm(double* initialReferencial, double theta);
/**
* Function to transpose a matrix
*
* Input pramaters:
* - input: A memory emplacement of size n*m
* - output: A memory emplacement of size m*n
* - n: Number of rows
* - m: Number of colums
*/
void utilities_transpose(double* input, double* output, int m, int n);
/**
* Function to do the product of two matrix
*
* Input pramaters:
* - mat1: A memory emplacement of size n*m
* - mat2: A memory emplacement of size m*p
* - res: A memory emplacement of size n*p
* - m: Number of rows mat1
* - n: Number of colums mat1 and rows mat2
* - p: Number of colums mat2
*/
void utilities_multiply(double* mat1, double* mat2, double* res, int m, int n, int p);
/**
* Print a matrix on stdo
*
* Input pramaters:
* - name: A name for the matrix
* - matrix: The matrix to print
* - m: Number of rows
* - n: Number of columns
*/
void utilities_printMatrix(char* name, double* matrix, int m, int n);
// Min - max number functions
double utilities_min(double a, double b);
void utilities_minImm(double* dest, double in);
double utilities_max(double a, double b);
void utilities_maxImm(double* dest, double in);
// Robot ID functions
int utilities_getRobotId();
int utilities_isInMyTeam(int robot);
int utilities_getTeamID(int robot);
......
......@@ -18,7 +18,6 @@
#define NB_SENSORS 8
#define DISTANCE_THRESHOLD 150
#define EPS 0.78
#ifdef IS_REAL_ROBOT
#include "./ircom/e_ad_conv.h"
......@@ -31,9 +30,9 @@ static WbDeviceTag ds[NB_SENSORS];
#endif
float ps_orientations[8] = {-0.3,-0.8,-1.57,3.64,2.64,1.57,0.8,0.3}; //proximity sensors orientations
float ps_weights[8] = {10, 8, 6, 3, 3,10, 14, 18};
//double ps_orientations[8] = {-0.3,-0.8,-1.57,3.64,2.64,1.57,0.8,0.3}; //proximity sensors orientations
double ps_orientations[8] = {-0.3,-0.8,-1.57,3.64,2.64,1.57,0.8,0.3}; //proximity sensors orientations
double ps_weights[8] = {20, 10, 5, 5, 5, 5, 15, 30};
void infrared_init(){
......@@ -51,17 +50,17 @@ void infrared_init(){
#endif
}
Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
Measurement* infrared_updateList(Measurement* measurementList, double* frontSpeedRatio){
double intensity[NB_SENSORS];
int i;
#ifdef IS_REAL_ROBOT
double sensorMean[NB_SENSORS];
for (i=0; i<NB_SENSORS; i++)
sensorMean[i]=0;
sensorMean[i]=0;
int n;
int n;
for(n=0;n<NB_SAMPLES; n++)
{
......@@ -75,63 +74,72 @@ Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
}
#else
double maxIntensity = 0;
int argMaxIntensity;
for(i=0;i<NB_SENSORS;i++)
{
intensity[i] = wb_distance_sensor_get_value(ds[i]);
if (intensity[i] > maxIntensity) {
maxIntensity = intensity[i];
argMaxIntensity = i;
}
}
#endif
for(i=0;i<NB_SENSORS;i++)
{
if(utilities_debug()) printf("sensor %d intensity at orientation %f = %f\n", i, ps_orientations[i], intensity);
//if(i == 3 || i == 4) continue;
if(utilities_debug()) printf("sensor %d intensity at orientation %f = %f\n", i, ps_orientations[i], intensity[i]);
if (intensity[i] > DISTANCE_THRESHOLD){
float distance = sqrt(1/intensity[i]);
float theta = ps_orientations[i];
/*if(measurementList){ // BUG ifno element in list
Measurement* element = measurementList;
do{
// Check if element overlaps
if( element->robotID != -1 &&
element->relativePosition[DISTANCE] < 0.12 &&
( element->relativePosition[THETA] > theta - EPS ||
element->relativePosition[THETA] < theta + EPS ) ){
isRobot = true;
break;
}
double distance = sqrt(1/intensity[i]);
double theta = ps_orientations[i];
element = element->next;
}while(element != NULL);
}
if(isRobot) continue;*/
*obstacle = 1;
/*if(i == 2 || i == 5) {
//utilities_minImm(frontSpeedRatio,0.8);
Measurement* temp = malloc(sizeof(Measurement));
Measurement* temp = malloc(sizeof(Measurement));
temp->next = measurementList;
temp->next = measurementList;
temp->robotID = -1;
temp->robotID = -1;
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->relativePosition[DISTANCE] = 0.02; // relative x pos
temp->relativePosition[THETA] = 3.14; // relative y pos
temp->relativePosition[DISTANCE] = distance; // relative x pos
temp->relativePosition[THETA] = theta; // relative y pos
//temp->weight = ps_weights[i]*2;
if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 )
temp->weight = 4;
else
temp->weight = 6;
temp->weight = 15;
measurementList = temp;
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
measurementList = temp;
}
}
return measurementList;
}*/
Measurement* temp = malloc(sizeof(Measurement));
temp->next = measurementList;
temp->robotID = -1;
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->relativePosition[DISTANCE] = distance; // relative x pos
temp->relativePosition[THETA] = theta; // relative y pos
temp->weight = ps_weights[i];
// //temp->weight = ps_weights[i]*2;
/*if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 )
temp->weight = 15;
else
temp->weight = 20;*/
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
measurementList = temp;
}
}
return measurementList;
}
......@@ -20,95 +20,16 @@
/***********************
* Local constants definition
*/
#define PRINT_MATRIX 1
//#define IS_TEST
#define ALPHA_FREE 0.7
#define ALPHA_OBST 0.5
#define MAX_ROBOTS 9
/***********************
* Local prototype
*/
/**
* Function to transpose a matrix
*
* Input pramaters:
* - input: A memory emplacement of size n*m
* - output: A memory emplacement of size m*n
* - n: Number of rows
* - m: Number of colums
*/
void transpose(float* input, float* output, int m, int n);
/**
* Function to do the product of two matrix
*
* Input pramaters:
* - mat1: A memory emplacement of size n*m
* - mat2: A memory emplacement of size m*p
* - res: A memory emplacement of size n*p
* - m: Number of rows mat1
* - n: Number of colums mat1 and rows mat2
* - p: Number of colums mat2
*/
void multiply(float* mat1, float* mat2, float* res, int m, int n, int p);
/**
* Print a matrix on stdo
*
* Input pramaters:
* - name: A name for the matrix
* - matrix: The matrix to print
* - m: Number of rows
* - n: Number of columns
*/
void printMatrix(char* name, float* matrix, int m, int n);
/***********************
* Methods implementation
*/
void transpose(float* input, float* output, int m, int n)
{
int i, j;
for(i=0; i<m; i++)
for(j=0; j<n; j++)
{
output[j*m+i] = input[i*n+j];
}
}
void multiply(float* mat1, float* mat2, float* res, int m, int n, int p)
{
int i, j, k;
for (i = 0; i < m; i++)
{
for (j = 0; j < p; j++)
{
res[i*p+j] = 0;
for (k = 0; k < n; k++)
res[i*p+j] += mat1[i*n+k]*mat2[k*p+j];
}
}
}
void printMatrix(char* name, float* matrix, int m, int n){
int i, j;
if(!PRINT_MATRIX || !utilities_debug()) return;
printf("Matrix %s (%d * %d)\n", name, m, n);
for(i= 0; i<m; i++){
for(j= 0; j<n; j++){
printf("%f\t", matrix[i*n+j]);
}
printf("\n");
}
}
#define NUMBER_ELEMENT_INT 1
float previousSpeedsRefAbs[NUMBER_ELEMENT_INT][2];
double previousSpeedsRefAbs[NUMBER_ELEMENT_INT][2];
int currentElement = 0;
void laplace_init(){
......@@ -119,7 +40,7 @@ void laplace_init(){
}
}
void laplace_compute(Measurement* objectsInSurround, float* updatedSpeedRefRob, float* weights, float* currentPositionRefRob){
void laplace_compute(Measurement* objectsInSurround, double* updatedSpeedRefRob, double* weights, double* currentPositionRefRob){
updatedSpeedRefRob[X] = 0;
updatedSpeedRefRob[Y] = 0;
......@@ -137,12 +58,12 @@ void laplace_compute(Measurement* objectsInSurround, float* updatedSpeedRefRob,
element = element->next;
}
float incidenceMatrix[(numberOfObjects+1) * numberOfObjects];
double incidenceMatrix[(numberOfObjects+1) * numberOfObjects];
int i, j;
for(i = 0; i < (numberOfObjects+1) * numberOfObjects; i++)
incidenceMatrix[i] = 0.;
float weightsMatrix[numberOfObjects*numberOfObjects];
double weightsMatrix[numberOfObjects*numberOfObjects];
for(i = 0; i < numberOfObjects * numberOfObjects; i++)
weightsMatrix[i] = 0.;
......@@ -152,7 +73,7 @@ void laplace_compute(Measurement* objectsInSurround, float* updatedSpeedRefRob,
incidenceMatrix[j] = 1;
incidenceMatrix[(j+1)*numberOfObjects+j] = -1;
}
printMatrix("incidenceMatrix", incidenceMatrix, numberOfObjects+1, numberOfObjects);
utilities_printMatrix("incidenceMatrix", incidenceMatrix, numberOfObjects+1, numberOfObjects);
int obstacleInSurround = 0;
......@@ -171,37 +92,33 @@ void laplace_compute(Measurement* objectsInSurround, float* updatedSpeedRefRob,
j++;
}
}
printMatrix("weightsMatrix", weightsMatrix, numberOfObjects, numberOfObjects);
// float alpha = (obstacleInSurround ? ALPHA_OBST : ALPHA_FREE);
utilities_printMatrix("weightsMatrix", weightsMatrix, numberOfObjects, numberOfObjects);
float incidenceMatrixT[numberOfObjects * (numberOfObjects+1)];
transpose(incidenceMatrix, incidenceMatrixT, numberOfObjects+1, numberOfObjects);
printMatrix("incidenceMatrixT", incidenceMatrixT, numberOfObjects, (numberOfObjects+1));
double incidenceMatrixT[numberOfObjects * (numberOfObjects+1)];
utilities_transpose(incidenceMatrix, incidenceMatrixT, numberOfObjects+1, numberOfObjects);
utilities_printMatrix("incidenceMatrixT", incidenceMatrixT, numberOfObjects, (numberOfObjects+1));
float IW[(numberOfObjects+1) * numberOfObjects];
multiply(incidenceMatrix, weightsMatrix, IW, numberOfObjects+1, numberOfObjects, numberOfObjects);
printMatrix("IW", IW, numberOfObjects+1, numberOfObjects);
double IW[(numberOfObjects+1) * numberOfObjects];
utilities_multiply(incidenceMatrix, weightsMatrix, IW, numberOfObjects+1, numberOfObjects, numberOfObjects);
utilities_printMatrix("IW", IW, numberOfObjects+1, numberOfObjects);
float L[(numberOfObjects+1) * (numberOfObjects+1)];
multiply(IW, incidenceMatrixT, L, numberOfObjects+1, numberOfObjects, numberOfObjects+1);
printMatrix("L", L, numberOfObjects+1, numberOfObjects+1);
double L[(numberOfObjects+1) * (numberOfObjects+1)];
utilities_multiply(IW, incidenceMatrixT, L, numberOfObjects+1, numberOfObjects, numberOfObjects+1);
utilities_printMatrix("L", L, numberOfObjects+1, numberOfObjects+1);
float velLaplaceRefRob[2] = {0, 0};
double velLaplaceRefRob[2] = {0, 0};
// Compute velocity of current robot with v = -L(x-B)
{
Measurement* nextObject = objectsInSurround;
int i = 1;
while(nextObject){
float distanceRefRob = nextObject->relativePosition[DISTANCE];
float thetaRefRob = nextObject->relativePosition[THETA];
float bias_trans[2];
double distanceRefRob = nextObject->relativePosition[DISTANCE];
double thetaRefRob = nextObject->relativePosition[THETA];
nextObject->biasPosition[X] /= 12;
nextObject->biasPosition[Y] /= 12;
......@@ -217,30 +134,10 @@ void laplace_compute(Measurement* objectsInSurround, float* updatedSpeedRefRob,
i++;
}
}
//updatedSpeedRefRob[X] = velLaplaceRefRob[X]/10;
//updatedSpeedRefRob[Y] = velLaplaceRefRob[Y]/10;
float velLaplaceRefAbs[2];
utilities_transformReferential(velLaplaceRefRob, velLaplaceRefAbs, -currentPositionRefRob[2]);
// Update of the speed in the table
previousSpeedsRefAbs[currentElement][X] = velLaplaceRefAbs[X]/10;
previousSpeedsRefAbs[currentElement][Y] = velLaplaceRefAbs[Y]/10;
currentElement = (++currentElement)%NUMBER_ELEMENT_INT;
float temp[2] = {0,0};
for(i = 0; i < NUMBER_ELEMENT_INT; i++){
temp[X] += previousSpeedsRefAbs[i][X];
temp[Y] += previousSpeedsRefAbs[i][Y];
}
temp[X] /= NUMBER_ELEMENT_INT;
temp[Y] /= NUMBER_ELEMENT_INT;
updatedSpeedRefRob[X] = velLaplaceRefRob[X]/10;
updatedSpeedRefRob[Y] = velLaplaceRefRob[Y]/10;
utilities_transformReferential(temp, updatedSpeedRefRob, currentPositionRefRob[2]);
if(utilities_getRobotId() == 8) printf("%f,%f\n", updatedSpeedRefRob[X], updatedSpeedRefRob[Y]);
//if(utilities_getRobotId() == 8) printf("%f,%f\n", updatedSpeedRefRob[X], updatedSpeedRefRob[Y]);
}
......@@ -53,16 +53,16 @@ void motors_init(){
#endif
}
void motors_transformSpeed(float* speed, int *speed_for_motors){
void motors_transformSpeed(double* speed, int *speed_for_motors){
float range = sqrtf(speed[X]*speed[X] + speed[Y]*speed[Y]);
double range = sqrtf(speed[X]*speed[X] + speed[Y]*speed[Y]);
float bearing = atan2( speed[Y], speed[X]);
double bearing = atan2( speed[Y], speed[X]);
if(utilities_debug()) printf("range = %f and bearing = %f\n", range, bearing);
float u = KU*range*cosf(bearing);
float w = KW*bearing;
double u = KU*range*cosf(bearing);
double w = KW*bearing;
speed_for_motors[LEFT] = (u - AXLE_LENGTH*w/2.0) * (1000.0 / WHEEL_RADIUS);
speed_for_motors[RIGHT]= (u + AXLE_LENGTH*w/2.0) * (1000.0 / WHEEL_RADIUS);
......@@ -80,7 +80,7 @@ void motors_transformSpeed(float* speed, int *speed_for_motors){
}
void motors_applySpeed(float* speed, int* appliedSpeed){
void motors_applySpeed(double* speed, int* appliedSpeed){
motors_transformSpeed(speed, appliedSpeed);
......
......@@ -36,7 +36,7 @@ static WbDeviceTag emitter;
#define MAX_BOUNDARY 0.3
// define bias matrix with at most 9 robots
float bias[10][2] = {{4,0},{3,1},{3,-1},{2,-2},
double bias[10][2] = {{4,0},{3,1},{3,-1},{2,-2},
{2,2},{2,0},{1,1},{1,-1},{0,0}};
/***********************
......
/********************************************************************************/
/* File: utilities.c */
/* Version: 1.0 */
/* Date: 11-Dec-19 */
/* Description: Controller for epucks */
/* */
/* Author: Hugo Aguettaz, Marie-Joe Stoeri & Nicolas Peslerbe */
/* Last revision:12-Oct-15 by Florian Maushart */
/********************************************************************************/
#include "inc/utilities.h"
#include "inc/measurements.h"
#include <math.h>
#include <stdio.h>
#define PRINT_FREQUENCY 1
#define PRINT_MATRIX 1
int enabledDebug = 0;
int numberPrint = 0;
// Debug functions
void utilities_changeDebugState(int enabledDebugTemp){
enabledDebug = enabledDebugTemp;
}
int utilities_debug(){
return ((numberPrint%50 == 0) && enabledDebug);
return ((numberPrint%PRINT_FREQUENCY == 0) && enabledDebug);
}
// Matrix operations fonctions
void utilities_transformReferential(double* initialReferencial, double* newReferencial, double theta){
newReferencial[X] = initialReferencial[X]*cosf(theta) + initialReferencial[Y]*sinf(theta);
newReferencial[Y] = - initialReferencial[X]*sinf(theta) + initialReferencial[Y]*cosf(theta);
}
void utilities_transformReferential(float* initialReferencial, float* newReferencial, float theta){
void utilities_transformReferentialImm(double* initialReferencial, double theta){
double newReferencial[2];
newReferencial[X] = initialReferencial[X]*cosf(theta) + initialReferencial[Y]*sinf(theta);
newReferencial[Y] = - initialReferencial[X]*sinf(theta) + initialReferencial[Y]*cosf(theta);
initialReferencial[X] = newReferencial[X];
initialReferencial[Y] = newReferencial[Y];
}
void utilities_transpose(double* input, double* output, int m, int n)
{
int i, j;
for(i=0; i<m; i++)
for(j=0; j<n; j++)
output[j*m+i] = input[i*n+j];
}
void utilities_multiply(double* mat1, double* mat2, double* res, int m, int n, int p)
{
int i, j, k;