/* take the definition of a camera and a point within the film plane of the camera and generate a RAY
x and y both have a range of -0.5 to +0.5 */
Ray *camGenerateRay(Camera *camera, double x, double y, Ray *ray){
Vec direction;
Vec cameraX, cameraY;
/* work out direction camera is pointing in */
vecSub(&(camera->lookAt), &(camera->at), &direction);
vecNormalise(&direction, &direction);
/* using that and DOWN, work out the camera axes and normalise */
vecProduct(&direction, &(camera->down), &cameraX);
vecProduct(&direction, &cameraX, &cameraY);
vecNormalise(&cameraX, &cameraX);
vecNormalise(&cameraY, &cameraY);
/* finally combine film offset and camera axes to work out film position */
vecScale(x * camera->width, &cameraX, &cameraX);
vecScale(y * camera->height, &cameraY, &cameraY);
vecScale(camera->depth, &direction, &direction);
vecAdd(&cameraX, &direction, &direction);
vecAdd(&cameraY, &direction, &direction);
rayInit(&(camera->at), &direction, ray);
return ray;
}
void applyTransitions(jrPlanet *pPlanet) {
if (pPlanet) {
float afForce[4];
vecInitDVec(afForce);
for (jrPlanet *pOtherPlanet = g_pHead; pOtherPlanet; pOtherPlanet = pOtherPlanet->m_pNext) {
if (pPlanet != pOtherPlanet && pOtherPlanet) {
float afDistance[3];
vecSub(pPlanet->afPosition, pOtherPlanet->afPosition, afDistance); //get distance
float fForce = g_fGravity * (pPlanet->fMass * pOtherPlanet->fMass) / (vecLength(afDistance));
float afNormaliseInput[4];
vecSub(pOtherPlanet->afPosition, pPlanet->afPosition, afNormaliseInput);
float afNormaliseOutput[4];
vecInit(afNormaliseOutput);
vecNormalise(afNormaliseInput, afNormaliseOutput);
afForce[0] = (afForce[0] + afNormaliseOutput[0]);
afForce[1] = (afForce[1] + afNormaliseOutput[1]);
afForce[2] = (afForce[2] + afNormaliseOutput[2]);
if (pPlanet->fMass > pOtherPlanet->fMass) {
if (detectCollision(pPlanet, pOtherPlanet, afDistance)) {
break;
}
}
}
}
// calc accV -> afForce/mass
pPlanet->afAcceleration[0] = afForce[0] / pPlanet->fMass;
pPlanet->afAcceleration[1] = afForce[1] / pPlanet->fMass;
pPlanet->afAcceleration[2] = afForce[2] / pPlanet->fMass;
// calc new pos
if (!pPlanet->m_bFixed) {
pPlanet->afPosition[0] = pPlanet->afPosition[0] + (pPlanet->afVelocity[0] * g_fVelocityMultiplier);
pPlanet->afPosition[1] = pPlanet->afPosition[1] + (pPlanet->afVelocity[1] * g_fVelocityMultiplier);
pPlanet->afPosition[2] = pPlanet->afPosition[2] + (pPlanet->afVelocity[2] * g_fVelocityMultiplier);
}
// calc new vel
pPlanet->afVelocity[0] = pPlanet->afVelocity[0] + pPlanet->afAcceleration[0];
pPlanet->afVelocity[1] = pPlanet->afVelocity[1] + pPlanet->afAcceleration[1];
pPlanet->afVelocity[2] = pPlanet->afVelocity[2] + pPlanet->afAcceleration[2];
// apply drag
pPlanet->afAcceleration[0] = pPlanet->afAcceleration[0] * g_fDamping;
pPlanet->afAcceleration[1] = pPlanet->afAcceleration[1] * g_fDamping;
pPlanet->afAcceleration[2] = pPlanet->afAcceleration[2] * g_fDamping;
// end
if (pPlanet->iHistoryCount < g_iHistoryVariableLength - 4) {
pPlanet->iHistoryCount = pPlanet->iHistoryCount + 3;
}
else {
pPlanet->iHistoryCount = 0;
}
pPlanet->afPositionHistory[pPlanet->iHistoryCount] = pPlanet->afPosition[0];
pPlanet->afPositionHistory[pPlanet->iHistoryCount + 1] = pPlanet->afPosition[1];
pPlanet->afPositionHistory[pPlanet->iHistoryCount + 2] = pPlanet->afPosition[2];
if (pPlanet->fRotationAngle > 359.0f) {
pPlanet->fRotationAngle = 0.0f;
}
else {
pPlanet->fRotationAngle = pPlanet->fRotationAngle += 1.0f;
}
}
}
myQuat_t getPosition(mpu9250_t dev)
{
imuData_t imuData;
if (getIMUData(dev, &imuData))
{
if (! validIMUData(imuData))
{
puts("invalid IMU data");
if (failureCount++ > 20)
{
puts("Error in IMU, restarting!");
identifyYourself("IMU failure");
if (! initialiseIMU(&dev))
{
puts("Could not initialise IMU! dying...");
exit(1);
}
}
return currentQ;
}
failureCount = 0;
/************************************************************************
* Get Gyro data that records current angular velocity and create an
* estimated quaternion representation of orientation using this
* velocity, the time interval between the last calculation and the
* previous orientation:
* change in position = velocity * time,
* new position = old position + change in position)
************************************************************************/
// get orientation as deduced by adding gyro measured rotational
// velocity (times time interval) to previous orientation.
double gx1 = (double)(imuData.gyro.x_axis);
double gy1 = (double)(imuData.gyro.y_axis);
double gz1 = (double)(imuData.gyro.z_axis);
double omega[3] = { gx1, gy1, gz1 }; // this will be in degrees/sec
//double omegaMagnitude = fabs(vecLength(omega));
uint32_t dt = imuData.ts - lastIMUData.ts; // dt in microseconds
lastIMUData = imuData;
myQuat_t gRot = makeQuatFromAngularVelocityTime(omega, dt/1000000.0);
myQuat_t gyroDeducedQ = quatMultiply(currentQ, gRot);
quatNormalise(&gyroDeducedQ);
if (! useGyroscopes)
{
makeIdentityQuat(&gyroDeducedQ);
}
if (! (useAccelerometers | useMagnetometers))
{
// return now with just the gyro data
currentQ = gyroDeducedQ;
lastIMUData = imuData;
return currentQ;
}
// get orientation from the gyro as Roll Pitch Yaw (ie Euler angles)
double gyroDeducedYPR[3];
quatToEuler(gyroDeducedQ, gyroDeducedYPR);
double currentYPR[3];
quatToEuler(currentQ, currentYPR);
// Calculate a roll/pich/yaw from the accelerometers and magnetometers.
/************************************************************************
* Get Accelerometer data that records gravity direction and create a
* 'measured' Quaternion representation of orientation (will only be
* pitch and roll, not yaw)
************************************************************************/
// get gravity direction from accelerometer
double ax1 = imuData.accel.x_axis / 1024.0;
double ay1 = imuData.accel.y_axis / 1024.0;
double az1 = imuData.accel.z_axis / 1024.0;
double downSensor[3] = { ax1, ay1, az1 };
if (fabs(vecLength(downSensor) - 0.98) > 0.5)
{
// excessive accelerometer reading, bail out
//printf("too much accel: %f ", fabs(vecLength(downSensor)));
//dumpVec(downSensor);
currentQ = gyroDeducedQ;
lastIMUData = imuData;
return currentQ;
}
vecNormalise(downSensor);
// The accelerometers can only measure pitch and roll (in the world reference
// frame), calculate the pitch and roll values to account for
// accelerometer readings, make yaw = 0.
double ypr[3];
double downX = downSensor[X_AXIS];
double downY = downSensor[Y_AXIS];
double downZ = downSensor[Z_AXIS];
// add a little X into Z when calculating roll to compensate for
// situations when pitching near vertical as Y, Z will be near 0 and
// the results will be unstable
double fiddledDownZ = sqrt(downZ*downZ + 0.001*downX*downX);
if (downZ <= 0) // compensate for loss of sign when squaring Z
fiddledDownZ *= -1.0;
ypr[ROLL] = atan2(downY, fiddledDownZ);
ypr[PITCH] = -atan2(downX, sqrt(downY*downY + downZ*downZ));
ypr[YAW] = 0; // yaw == 0, accelerometer cannot measure yaw
if (! useAccelerometers)
{
// if no accelerometers, use roll and pitch values from gyroscope
// derived orientation
ypr[PITCH] = gyroDeducedYPR[PITCH];
ypr[ROLL] = gyroDeducedYPR[ROLL];
ypr[YAW] = gyroDeducedYPR[YAW];
}
else if (vecLength(downSensor) == 0)
{
puts("weightless?");
// something wrong! - weightless??
ypr[PITCH] = gyroDeducedYPR[PITCH];
ypr[ROLL] = gyroDeducedYPR[ROLL];
ypr[YAW] = gyroDeducedYPR[YAW];
}
if (! useMagnetometers)
{
ypr[YAW] = gyroDeducedYPR[YAW];
}
myQuat_t accelQ = eulerToQuat(ypr);
myQuat_t invAccelQ = quatConjugate(accelQ);
/************************************************************************
* Get Magnetometer data that records north direction and create a
* 'measured' Quaternion representation of orientation (will only be
* yaw)
************************************************************************/
// get magnetometer indication of North
double mx1 = (double)(imuData.mag.x_axis - magHardCorrection[X_AXIS]);
double my1 = (double)(imuData.mag.y_axis - magHardCorrection[Y_AXIS]);
double mz1 = (double)(imuData.mag.z_axis - magHardCorrection[Z_AXIS]);
mx1 *= magSoftCorrection[X_AXIS];
my1 *= magSoftCorrection[Y_AXIS];
mz1 *= magSoftCorrection[Z_AXIS];
// NB MPU9250 has different XYZ axis for magnetometers than for gyros
// and accelerometers so juggle x,y,z here...
double magV[3] = { my1, mx1, -mz1 };
vecNormalise(magV);
// make quaternion representing mag readings
myQuat_t magQ = quatFromValues(0, magV[X_AXIS], magV[Y_AXIS], magV[Z_AXIS]);
if (vecLength(magV) == 0)
{
// something wrong! - no magnetic field??
//puts("not on earth?");
magQ = quatFromValues(1, 0, 0, 0);
}
// the magnetometers can only measure yaw (in the world reference
// frame), adjust the yaw of the roll/pitch/yaw values calculated
// earlier to account for the magnetometer readings.
if (useMagnetometers)
{
// pitch and roll the mag direction using accelerometer info to
// create a quaternion that represents the IMU as if it was
// horizontal (so we can get pure yaw)
myQuat_t horizQ = quatMultiply(accelQ, magQ);
horizQ = quatMultiply(horizQ, invAccelQ);
// calculate pure yaw
ypr[YAW] = -atan2(horizQ.y, horizQ.x);
}
// measuredQ is the orientation as measured using the
// accelerometer/magnetometer readings
myQuat_t measuredQ = eulerToQuat(ypr);
// check measured and deduced orientations are not wildly different (eg
// due to a 360d alias flip) and adjust if problem...
measuredQ = adjustForCongruence(measuredQ, gyroDeducedQ);
/*************************************************************************
* The gyro estimated orientation will be smooth and precise over short
* time intervals but small errors will accumulate causing it to
* drift over time. The 'measured' orientation as obtained from the
* magnetometer and accelerometer will be jumpy but will always average
* around the correct orientation.
*
* Take the mag and accel 'measured' orientation and the gyro
* 'estimated' orientation and create a new 'current' orientation that
* is the estimated orientation corrected slightly towards the measured
* orientation. That way we get the smooth precision of the gyros but
* eliminate the accumulation of drift errors.
************************************************************************/
if (useGyroscopes)
{
// the new orientation is the gyro deduced position corrected
// towards the accel/mag measured position using interpolation
// between quaternions.
currentQ = slerp(gyroDeducedQ, measuredQ, 0.02);
}
else
{
currentQ = measuredQ;
}
if (! isQuatValid(currentQ))
{
puts("error in quaternion, reseting orientation...");
displayData(imuData);
dumpQuat(currentQ);
makeIdentityQuat(¤tQ);
}
}
return currentQ;
}
