void Plante::Racine::actualiser (sf::Time deltaT )
{
double rad = m_angle*PI/180;
float rayonParent = m_parent->m_shape.getRadius();
sf::Vector2f vecNorm ((float)cos( rad ), (float)sin( rad ));
sf::Vector2f pos ( vecNorm.x * rayonParent, vecNorm.y * rayonParent );
m_shape.setOrigin( m_shape.getRadius(), m_shape.getRadius() );
setPosition ( pos.x, pos.y );
}
int main(int argc, char** argv) {
if(argc != 3) {
printf("Usage: ./gram_schmidt vector_length number_of_vectors\n");
exit(EXIT_FAILURE);
}
double** v;
double **q;
double temp_norm, sigma;
int row = atoi(argv[1]);
//int col = row;
int col = atoi(argv[2]);
if(row < col) {
printf("It is not possible to create an orthogonal base with such values. \n");
exit(-1);
}
int k,i,j,ttime;
v = alloc_matrix(row, col);
q = alloc_matrix(row, col);
// initializes v with random values
srand(time(NULL));
init(v, row, col);
// compute
ttime=timer();
for(i=0; i<row; i++) {
temp_norm = vecNorm(v[i],col);
for (k=0; k<col; k++)
q[i][k] = v[i][k]/temp_norm;
#pragma omp parallel for private(temp_norm, k, j, sigma) schedule(dynamic)
for(j=i+1; j<row; j++) {
sigma = scalarProd(q[i], v[j], col);
for(k=0; k<col; k++)
v[j][k] -=sigma*q[i][k];
}
}
ttime=timer()-ttime;
printf("Time: %f \n",ttime/1000000.0);
printf("Check orthogonality: %e \n",scalarProd(q[col/2], q[col/3], row));
}
void calibrate()
{
// TODO: wait for things to stabilise
//printf("Calibrating\n");
Compass_ReadAccAvg(Zaxis, 1000);
vecNorm(Zaxis);
//printf("Z: %9.3f %9.3f %9.3f\n", Zaxis[0], Zaxis[1], Zaxis[2]);
Compass_ReadMag(Xaxis);
vecNorm(Xaxis);
//printf("X: %9.3f %9.3f %9.3f\n", Xaxis[0], Xaxis[1], Xaxis[2]);
vecCross(Yaxis, Zaxis, Xaxis);
vecNorm(Yaxis);
vecCross(Xaxis, Yaxis, Zaxis);
vecNorm(Xaxis);
Gyro_ReadAngRateAvg(zeroAngRate, 100);
//printf("X: %9.3f %9.3f %9.3f\n", Xaxis[0], Xaxis[1], Xaxis[2]);
//printf("Y: %9.3f %9.3f %9.3f\n", Yaxis[0], Yaxis[1], Yaxis[2]);
//printf("Z: %9.3f %9.3f %9.3f\n", Zaxis[0], Zaxis[1], Zaxis[2]);
}
void calibrate()
{
// at rest the accelerometer reports an acceleration straight upwards
// due to gravity. use this as our Z axis
Compass_ReadAccAvg(Zaxis, 500);
vecNorm(Zaxis);
// the magnetic strength is greatest towards magnetic north. use this as
// our X axis
Compass_ReadMagAvg(Xaxis, 10);
vecNorm(Xaxis);
// create a Y axis perpendicular to the X and Z axes
vecCross(Yaxis, Zaxis, Xaxis);
vecNorm(Yaxis);
// ensire that the X axis is actually perpendicular to the Y and Z axes
vecCross(Xaxis, Yaxis, Zaxis);
vecNorm(Xaxis);
// calibrate the zero error on the gyroscope
Gyro_ReadAngRateAvg(zeroAngRate, 100);
}
void GLRender::moveCamera(int value)
{
startStoreViewImageAndPose = 1;
// define camera coordinate system
GLRenderVec x_axis, y_axis, z_axis, up;
x_axis.x = 1; x_axis.y = 0; x_axis.z = 0;
y_axis.x = 0; y_axis.y = 1; y_axis.z = 0;
z_axis.x = 0; z_axis.y = 0; z_axis.z = 1;
up.x = 0; up.y = 0; up.z = 1;
cv::Matx33f cs = cv::Matx33f::eye();
// change scale
if(m_radius<=m_maxRadius)
{
// change latitude
if(m_latitude<=m_maxLatitude)
{
float lati_radian = m_latitude*PI/180;
// when at north polar or south polar, ignore the longitude change
if(m_latitude==90.0f || m_latitude==-90.0f)
{
// camera origin location
m_translation[0] = 0.0f;
m_translation[1] = 0.0f;
m_translation[2] = m_radius;
// change inplane rotation
if(m_inplane<=m_maxInplane)
{
// view direction axis
float axis[3] = {-m_translation[0]/m_radius, -m_translation[1]/m_radius, -m_translation[2]/m_radius};
// calculate camera coordinate system axises
z_axis.x = -axis[0]; z_axis.y = -axis[1]; z_axis.z = -axis[2];
x_axis.x = 1; x_axis.y = 0; x_axis.z = 0;
y_axis.x = 0; y_axis.y = 1; y_axis.z = 0;
cs(0,0) = x_axis.x; cs(0,1) = y_axis.x; cs(0,2) = z_axis.x;
cs(1,0) = x_axis.y; cs(1,1) = y_axis.y; cs(1,2) = z_axis.y;
cs(2,0) = x_axis.z; cs(2,1) = y_axis.z; cs(2,2) = z_axis.z;
// define inplate rotation matrix
// rotation alone the z-axis (i.e. the inversed view direction)
float inplane_radian = m_inplane*PI/180;
cv::Matx33f inplaneRot = cv::Matx33f::eye();
inplaneRot(0,0) = cos(inplane_radian); inplaneRot(0,1) = -sin(inplane_radian);
inplaneRot(1,0) = sin(inplane_radian); inplaneRot(1,1) = cos(inplane_radian);
cs = cs*inplaneRot;
// represent the camera coordinate system using Rodrigues formate
cv::Matx31f cs_rodrigues;
cv::Rodrigues(cs, cs_rodrigues);
m_rotation[0] = cs_rodrigues(0);
m_rotation[1] = cs_rodrigues(1);
m_rotation[2] = cs_rodrigues(2);
// go to next inplane rotation state
m_inplane+=m_inplaneStep;
++viewCounter;
// animation
glutPostRedisplay();
glutTimerFunc(10, moveCamera, 1);
return;
}
}
else
{
// change longitude
if(m_longitude<=m_maxLongitude)
{
float longi_radian = m_longitude*PI/180;
// define camera origin location
m_translation[0] = m_radius * cos(lati_radian) * cos(longi_radian);
m_translation[1] = m_radius * cos(lati_radian) * sin(longi_radian);
m_translation[2] = m_radius * sin(lati_radian);
// change inplane rotation
if(m_inplane<=m_maxInplane)
{
// view direction axis
float axis[3] = {-m_translation[0]/m_radius, -m_translation[1]/m_radius, -m_translation[2]/m_radius};
// calculate camera coordinate system axises
z_axis.x = -axis[0]; z_axis.y = -axis[1]; z_axis.z = -axis[2];
x_axis = vecCross(up, z_axis);
float x_axis_norm = vecNorm(x_axis);
if(x_axis_norm>0)
x_axis.x /= x_axis_norm; x_axis.y /= x_axis_norm; x_axis.z /= x_axis_norm;
y_axis = vecCross(z_axis, x_axis);
float y_axis_norm = vecNorm(y_axis);
if(y_axis_norm>0)
y_axis.x /= y_axis_norm; y_axis.y /= y_axis_norm; y_axis.z /= y_axis_norm;
cs(0,0) = x_axis.x; cs(0,1) = y_axis.x; cs(0,2) = z_axis.x;
cs(1,0) = x_axis.y; cs(1,1) = y_axis.y; cs(1,2) = z_axis.y;
cs(2,0) = x_axis.z; cs(2,1) = y_axis.z; cs(2,2) = z_axis.z;
// define inplate rotation matrix
// rotation alone the z-axis (i.e. the inversed view direction)
float inplane_radian = m_inplane*PI/180;
cv::Matx33f inplaneRot = cv::Matx33f::eye();
inplaneRot(0,0) = cos(inplane_radian); inplaneRot(0,1) = -sin(inplane_radian);
inplaneRot(1,0) = sin(inplane_radian); inplaneRot(1,1) = cos(inplane_radian);
cs = cs*inplaneRot;
// represent the camera coordinate system using Rodrigues formate
cv::Matx31f cs_rodrigues;
cv::Rodrigues(cs, cs_rodrigues);
m_rotation[0] = cs_rodrigues(0);
m_rotation[1] = cs_rodrigues(1);
m_rotation[2] = cs_rodrigues(2);
// go to next inplane rotation state
m_inplane+=m_inplaneStep;
++viewCounter;
// animation
glutPostRedisplay();
glutTimerFunc(10, moveCamera, 1);
return;
}
m_longitude+=m_longitudeStep;
m_inplane = m_minInplane;
// animation
glutTimerFunc(10, moveCamera, 1);
return;
}
}
m_latitude+=m_latitudeStep;
m_longitude = m_minLongitude;
m_inplane = m_minInplane;
// animation
glutTimerFunc(10, moveCamera, 1);
return;
}
m_radius+=m_radiusStep;
m_latitude = m_minLatitude;
m_longitude = m_minLongitude;
m_inplane = m_minInplane;
// animation
glutTimerFunc(10, moveCamera, 1);
return;
}
// done the camera viewpoints sampling and exit the main loop
// close the opengl context
poseOutFile.close();
glutLeaveMainLoop();
}
int main()
{
setvbuf(stdout, NULL, _IONBF, 0);
setvbuf(stderr, NULL, _IONBF, 0);
/*!< At this stage the microcontroller clock setting is already configured,
this is done through SystemInit() function which is called from startup
file (startup_stm32f30x.s) before to branch to application main.
To reconfigure the default setting of SystemInit() function, refer to
system_stm32f30x.c file
*/
/* SysTick end of count event each 10ms */
RCC_GetClocksFreq(&RCC_Clocks);
SysTick_Config(RCC_Clocks.HCLK_Frequency / 100);
/* initialise USART1 debug output (TX on pin PA9 and RX on pin PA10) */
USART1_Init();
//printf("Starting\n");
USART1_flush();
/*
printf("Initialising USB\n");
USBHID_Init();
printf("Initialising USB HID\n");
Joystick_init();
*/
/* Initialise LEDs */
//printf("Initialising LEDs\n");
int i;
for (i = 0; i < 8; ++i) {
STM_EVAL_LEDInit(leds[i]);
STM_EVAL_LEDOff(leds[i]);
}
/* Initialise gyro */
//printf("Initialising gyroscope\n");
Gyro_Init();
/* Initialise compass */
//printf("Initialising compass\n");
Compass_Init();
Delay(100);
calibrate();
int C = 0, noAccelCount = 0;
while (1) {
float *acc = accs[C&1],
*prevAcc = accs[(C&1)^1],
*vel = vels[C&1],
*prevVel = vels[(C&1)^1],
*pos = poss[C&1],
*prevPos = poss[(C&1)^1],
*angRate = angRates[C&1],
*prevAngRate = angRates[(C&1)^1],
*ang = angs[C&1],
*prevAng = angs[(C&1)^1],
*mag = mags[C&1],
*prevmag = mags[(C&1)^1];
/* Wait for data ready */
#if 0
Compass_ReadAccAvg(acc, 10);
vecMul(axes, acc);
//printf("X: %9.3f Y: %9.3f Z: %9.3f\n", acc[0], acc[1], acc[2]);
float grav = acc[2];
acc[2] = 0;
if (noAccelCount++ > 50) {
for (i = 0; i < 2; ++i) {
vel[i] = 0;
prevVel[i] = 0;
}
noAccelCount = 0;
}
if (vecLen(acc) > 50.f) {
for (i = 0; i < 2; ++i) {
vel[i] += prevAcc[i] + (acc[i]-prevAcc[i])/2.f;
pos[i] += prevVel[i] + (vel[i]-prevVel[i])/2.f;
}
noAccelCount = 0;
}
C += 1;
if (((C) & 0x7F) == 0) {
printf("%9.3f %9.3f %9.3f %9.3f %9.3f\n", vel[0], vel[1], pos[0], pos[1], grav);
//printf("%3.1f%% %d %5.1f %6.3f\n", (float) timeReadI2C*100.f / totalTime, C, (float) C*100.f / (totalTime), grav);
}
#endif
Compass_ReadMagAvg(mag, 2);
vecMul(axes, mag);
float compassAngle = atan2f(mag[1], mag[0]) * 180.f / PI;
if (compassAngle > 180.f) compassAngle -= 360.f;
//vecNorm(mag);
Gyro_ReadAngRateAvg(mag, 2);
printf("%6.3f:%6.3f,%6.3f,%6.3f\n", compassAngle, mag[0], mag[1], mag[2]);
#if 0
Gyro_ReadAngRateAvg(angRate, 2);
angRate[0] *= 180.f / PI;
angRate[1] *= 180.f / PI;
angRate[2] *= 180.f / PI;
float s[3] = {sin(angRate[0]), sin(angRate[1]), sin(angRate[2])};
float c[3] = {cos(angRate[0]), cos(angRate[1]), cos(angRate[2])};
float gyroMat[3][3] = {
{c[0]*c[1], c[0]*s[1], -s[1]},
{c[0]*s[1]*s[2]-s[0]*c[2], c[0]*c[2]+s[0]*s[1]*s[2], c[1]*s[2]},
{c[0]*s[1]*c[2]+s[0]*s[2], -c[0]*s[2]+s[0]*s[1]*c[2], c[1]*c[2]}};
/*
float gyroWorldMat[3][3];
vecMulMatTrans(gyroWorldMat, axes, gyroMat);
*ang = gyroWorldMat[2][0];
*ang += gyroWorldMat[2][1];
*ang += gyroWorldMat[2][2];
*ang /= 300.f;
static const float ANGALPHA = 0.0f;
*ang += ANGALPHA*(compassAngle - *ang);
*/
float rotObsVec[3];
memcpy(rotObsVec, axes[0], sizeof(rotObsVec));
vecMul(gyroMat, rotObsVec);
vecMul(axes, rotObsVec);
rotObsVec[2] = 0.f;
vecNorm(rotObsVec);
float angDelta = acos(rotObsVec[0]);
if (((++C) & 0x7) == 0) {
printf("%6.3f %6.3f %6.3f %6.3f\n", rotObsVec[0], rotObsVec[1], rotObsVec[2], angDelta);
}
#endif
#if 0
float angRate[3];
/* Read Gyro Angular data */
Gyro_ReadAngRate(angRate);
printf("X: %f Y: %f Z: %f\n", angRate[0], angRate[1], angRate[2]);
float MagBuffer[3] = {0.0f}, AccBuffer[3] = {0.0f};
float fNormAcc,fSinRoll,fCosRoll,fSinPitch,fCosPitch = 0.0f, RollAng = 0.0f, PitchAng = 0.0f;
float fTiltedX,fTiltedY = 0.0f;
Compass_ReadMag(MagBuffer);
Compass_ReadAcc(AccBuffer);
for(i=0;i<3;i++)
AccBuffer[i] /= 100.0f;
fNormAcc = sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));
fSinRoll = -AccBuffer[1]/fNormAcc;
fCosRoll = sqrt(1.0-(fSinRoll * fSinRoll));
fSinPitch = AccBuffer[0]/fNormAcc;
fCosPitch = sqrt(1.0-(fSinPitch * fSinPitch));
if ( fSinRoll >0)
{
if (fCosRoll>0)
{
RollAng = acos(fCosRoll)*180/PI;
}
else
{
RollAng = acos(fCosRoll)*180/PI + 180;
}
}
else
{
if (fCosRoll>0)
{
RollAng = acos(fCosRoll)*180/PI + 360;
}
else
{
RollAng = acos(fCosRoll)*180/PI + 180;
}
}
if ( fSinPitch >0)
{
if (fCosPitch>0)
{
PitchAng = acos(fCosPitch)*180/PI;
}
else
{
PitchAng = acos(fCosPitch)*180/PI + 180;
}
}
else
{
if (fCosPitch>0)
{
PitchAng = acos(fCosPitch)*180/PI + 360;
}
else
{
PitchAng = acos(fCosPitch)*180/PI + 180;
}
}
if (RollAng >=360)
{
RollAng = RollAng - 360;
}
if (PitchAng >=360)
{
PitchAng = PitchAng - 360;
}
fTiltedX = MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch;
fTiltedY = MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch;
HeadingValue = (float) ((atan2f((float)fTiltedY,(float)fTiltedX))*180)/PI;
printf("Compass heading: %f\n", HeadingValue);
#endif
}
return 1;
}
void
LOCA::Extended::MultiVector::norm(std::vector<double>& result,
NOX::Abstract::Vector::NormType type) const
{
// Make sure result vector is of appropriate size
if (result.size() != static_cast<unsigned int>(numColumns))
result.resize(numColumns);
// Zero out result vector
for (int i=0; i<numColumns; i++)
result[i] = 0.0;
// Intermediate vector to hold norms of a multivector
std::vector<double> vecNorm(result);
switch (type) {
case NOX::Abstract::Vector::MaxNorm:
// compute max norm of all multivecs
for (int i=0; i<numMultiVecRows; i++) {
multiVectorPtrs[i]->norm(vecNorm, type);
for (int j=0; j<numColumns; j++) {
if (result[j] < vecNorm[j])
result[j] = vecNorm[j];
}
}
// compute max norm of each scalar vector column
for (int j=0; j<numColumns; j++) {
for (int i=0; i<numScalarRows; i++)
if (result[j] < (*scalarsPtr)(i,j))
result[j] = (*scalarsPtr)(i,j);
}
break;
case NOX::Abstract::Vector::OneNorm:
// compute one-norm of all multivecs
for (int i=0; i<numMultiVecRows; i++) {
multiVectorPtrs[i]->norm(vecNorm, type);
for (int j=0; j<numColumns; j++) {
result[j] += vecNorm[j];
}
}
// compute one-norm of each scalar vector column
for (int j=0; j<numColumns; j++) {
for (int i=0; i<numScalarRows; i++)
result[j] += fabs((*scalarsPtr)(i,j));
}
break;
case NOX::Abstract::Vector::TwoNorm:
default:
// compute two-norm of all multivecs
for (int i=0; i<numMultiVecRows; i++) {
multiVectorPtrs[i]->norm(vecNorm, type);
for (int j=0; j<numColumns; j++) {
result[j] += vecNorm[j]*vecNorm[j];
}
}
// compute two-norm of each scalar vector column
for (int j=0; j<numColumns; j++) {
for (int i=0; i<numScalarRows; i++)
result[j] += (*scalarsPtr)(i,j) * (*scalarsPtr)(i,j);
result[j] = sqrt(result[j]);
}
break;
}
}
