135 lines
3.8 KiB
C++
135 lines
3.8 KiB
C++
#include "../headers/sensore.hpp"
|
|
|
|
|
|
Sensore::Sensore(rb::Vector3 coords, _Float16 mass){
|
|
rb::Vector3 com = {sensore_Dim.x/2,sensore_Dim.z/2, sensore_Dim.y/2};
|
|
body = rb::rigidbody(coords, com, mass, sensore_Dim.z/2);
|
|
color = sensore_Col;
|
|
globalPos = {0,0,0};
|
|
initialize_shapes(sensore_Dim);
|
|
}
|
|
|
|
Sensore::Sensore(rb::Vector3 coords, _Float16 mass, unsigned int* st, std::vector<std::vector<float>> data) : Sensore(coords, mass){
|
|
dataPos = st;
|
|
initCSV(data);
|
|
}
|
|
|
|
|
|
Sensore::~Sensore(){
|
|
delete shapeXZ;
|
|
delete shapeYZ;
|
|
}
|
|
|
|
void Sensore::initCSV(std::vector<std::vector<float>> data){
|
|
//timestamp_ns, wx, wy, wz, ax, ay, az, gx, gy, gz
|
|
if (data.size() < 10) throw "Sensor data empty";
|
|
float stTime = int64_t( data[0][0] ) ;
|
|
|
|
for (std::vector<float> row : data){
|
|
timeData.push_back(int64_t( row[0] ) - stTime);
|
|
|
|
std::vector<float> tmpR = {row[2],row[3],row[1]};
|
|
std::vector<float> tmpA = {row[5],row[6],row[4]};
|
|
std::vector<float> tmpG = {-row[8],-row[9],-row[7]};
|
|
|
|
|
|
rotData.push_back(tmpR);
|
|
accData.push_back(tmpA);
|
|
gData.push_back(tmpG);
|
|
}
|
|
|
|
//trovo il modulo di g facendo la media del modulo nei primi 1000 campioni
|
|
gModule = 0;
|
|
int nCampioni = int(data.size())>1000 ? 1000 : 10;
|
|
for(int i = 0; i<nCampioni ;i++) {
|
|
gModule += sqrt(pow(gData[i][0],2)+pow(gData[i][1],2)+pow(gData[i][2],2));
|
|
}
|
|
gModule = gModule / 1000;
|
|
|
|
}
|
|
|
|
|
|
void Sensore::update(sf::Clock cl, float multiplier){
|
|
|
|
//calcolo la posizione e velocità
|
|
if (*dataPos >= gData.size()) *dataPos = gData.size()-1;
|
|
|
|
calcRotWithG(*dataPos);
|
|
|
|
|
|
body.setAcc(rb::Vector3{accData[*dataPos]});
|
|
body.step(cl, multiplier);
|
|
|
|
}
|
|
|
|
sf::Shape* Sensore::draw(ReferencePlane plane){
|
|
|
|
rb::Vector3 tmpRot = body.getRot();
|
|
rb::Vector3 tmpPos = body.getPos();
|
|
|
|
switch (plane)
|
|
{
|
|
case ReferencePlane::XZ : case ReferencePlane::XZN:
|
|
{
|
|
|
|
sf::Shape* shape = shapeXZ;
|
|
shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
|
|
shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
|
|
shape->setFillColor(color*sf::Color(255,255,255,transparency*255));
|
|
shape->setScale({plane == ReferencePlane::XZ ? float(1.0) : float(-1.0),1});
|
|
return shape;}
|
|
break;
|
|
|
|
case ReferencePlane::YZ:
|
|
{
|
|
sf::Shape* shape = shapeYZ;
|
|
shape->setRotation(sf::Angle(sf::radians(tmpRot[0])));
|
|
shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
|
|
shape->setFillColor(color*sf::Color(255,255,255,transparency*255));
|
|
shape->setScale({direction == Direction::R ? float(1.0) : float(-1.0),1});
|
|
return shape;}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return nullptr;
|
|
}
|
|
|
|
|
|
void Sensore::calcRotWithG(unsigned int index){ // calcolo rotazione con valori della gravità
|
|
int dir = direction == Direction::R ? -1 : 1;
|
|
std::vector<float> grav = gData[index];
|
|
|
|
//x = mod * cosX -> mod = x/cosx -> cosx = x/mod
|
|
|
|
float tmpSinX = -grav[0] / gModule;
|
|
float tmpSinY = -grav[1] / gModule;
|
|
float tmpSinZ = -grav[2] / gModule;
|
|
|
|
float tmpAX = acos(dir*tmpSinX);
|
|
float tmpAY = acos(dir*tmpSinY);
|
|
float tmpAZ = acos(tmpSinZ);
|
|
|
|
body.setRot(rb::Vector3{tmpAY, tmpAX, tmpAZ });
|
|
|
|
}
|
|
|
|
float Sensore::getZ_Acc(){
|
|
//int id = *dataPos;
|
|
float tmpAcc = 0;
|
|
|
|
rb::Vector3 acc = body.getAcc();
|
|
rb::Vector3 rot = body.getRot();
|
|
|
|
float modAcc = sqrt(pow(acc[0],2)+pow(acc[1],2)+pow(acc[2],2));
|
|
|
|
float zAcc = cos(rot[2]) * modAcc;
|
|
|
|
//dipende se il sensore conta la gravità nell'accelerazione sugli assi
|
|
tmpAcc = zAcc - gModule;
|
|
//tmpAcc = gModule - sqrt(pow(gData[id][0],2)+pow(gData[id][1],2)+pow(gData[id][2],2));
|
|
return tmpAcc;
|
|
}
|
|
/////////////// cinematica inversa
|