int rotatePositionOfObjectTrajectory(struct VirtualStream * stream,unsigned int ObjID,unsigned int FrameIDToReturn,float *x,float *y,float *z,float *angleDegrees)
{
FrameIDToReturn=FrameIDToReturn%stream->object[ObjID].numberOfFrames;
if (!accessOfObjectPositionIsOk(stream,ObjID,FrameIDToReturn)) { return 0; }
if ( stream->object[ObjID].frame[FrameIDToReturn].isQuaternion )
{
fprintf(stderr,"rotatePositionOfObjectTrajectory doing a quaternion rotation %0.2f %0.2f %0.2f , angle %0.2f..\n",*x,*y,*z,*angleDegrees);
double quaternion[4]={0};
quaternion[0]=stream->object[ObjID].frame[FrameIDToReturn].rot1;
quaternion[1]=stream->object[ObjID].frame[FrameIDToReturn].rot2;
quaternion[2]=stream->object[ObjID].frame[FrameIDToReturn].rot3;
quaternion[3]=stream->object[ObjID].frame[FrameIDToReturn].rot4;
//-----------
quaternionRotate(quaternion,*x,*y,*z,*angleDegrees,0);
//-----------
stream->object[ObjID].frame[FrameIDToReturn].rot1=quaternion[0];
stream->object[ObjID].frame[FrameIDToReturn].rot2=quaternion[1];
stream->object[ObjID].frame[FrameIDToReturn].rot3=quaternion[2];
stream->object[ObjID].frame[FrameIDToReturn].rot4=quaternion[3];
} else
{
fprintf(stderr,"rotatePositionOfObjectTrajectory doing an euler rotation %0.2f %0.2f %0.2f , angle %0.2f..\n",*x,*y,*z,*angleDegrees);
if ( (*x==1.0) && (*y==0.0) && (*z==0.0) ) { stream->object[ObjID].frame[FrameIDToReturn].rot1+=*angleDegrees; } else
if ( (*x==0.0) && (*y==1.0) && (*z==0.0) ) { stream->object[ObjID].frame[FrameIDToReturn].rot2+=*angleDegrees; } else
if ( (*x==0.0) && (*y==0.0) && (*z==1.0) ) { stream->object[ObjID].frame[FrameIDToReturn].rot3+=*angleDegrees; }
}
return 0;
}
int testRotation(double * quaternions , double * euler , double rX,double rY,double rZ)
{
unsigned int i=0;
signed int inc=10;
for (i=0; i<5; i++)
{
quaternionRotate(quaternions,rX,rY,rZ, inc,qXqYqZqW);
quaternions2Euler(euler,quaternions,qXqYqZqW);
printf("Step %u %d | %f %f %f \n",i,inc,euler[0],euler[1],euler[2]);
}
inc=-10;
for (i=0; i<5; i++)
{
quaternionRotate(quaternions,rX,rY,rZ, inc,qXqYqZqW);
quaternions2Euler(euler,quaternions,qXqYqZqW);
printf("Step %u %d | %f %f %f \n",i,inc,euler[0],euler[1],euler[2]);
}
return 1;
}
