double Line::distancePointToLine(Matrix<double> pos)
{
double l = length();
Matrix<double> eff_pos_a = effectivePosA();
Matrix<double> eff_pos_b = effectivePosB();
if (l <= 0)
{
return vecLength(pos - eff_pos_a);
}
const double t = vecDot(pos - eff_pos_a, eff_pos_b - eff_pos_a) / l; // This is readily shown by drawing out the relevant vectors
if (t < 0.0)
{
return vecLength(pos - eff_pos_a); // Beyond the 'eff_pos_a' end of the segment
}
else if (t > 1.0)
{
return vecLength(pos - eff_pos_b); // Beyond the 'eff_pos_b' end of the segment
}
Matrix<double> projection = eff_pos_a + t * (eff_pos_b - eff_pos_a); // Projection falls on the segment
return vecLength(pos - projection);
}
void circlePlyAreaCT(Ball *inBall,LevelInfo *lvlInfo,float *padStepAng)
{
vec2f P;
vec2f center = lvlInfo->center;
int counter = 0;
float vp, tan;
float cx, cy;
float lengthP;
float radius = lvlInfo->radius;
float cRadLength = sqrtf((radius - inBall->ballRadius) * (radius - inBall->ballRadius));
cx = center.x;
cy = center.y;
/* bound ball off the sides of circle play area */
P.x = inBall->pos.x - cx;
P.y = inBall->pos.y - cy;
lengthP = vecLength(&P);
if (lengthP >= cRadLength) {
normalize (&P,1);
tan = atan2(P.y,P.x) * 180/M_PI;
/* if hit in the range of 210 to 310 deg, play loss 1 life */
if (tan > -150.0 && tan < -30.0f) {
lvlInfo->life += lvlInfo->life > 0 ? -1 : 5;
inBall->pos.x = center.x;
inBall->pos.y = center.y;
inBall->ballVel.y = -1.4;
inBall->ballVel.x = 0.0f;
*padStepAng = 0.0f;
return;
}
// Find projection
vp = dotProduct(inBall->ballVel,P);
inBall->ballVel.x -= 2 * vp * P.x;
inBall->ballVel.y -= 2 * vp * P.y;
normalize (&inBall->ballVel,2);
P.x = cx - inBall->pos.x;
P.y = cy - inBall->pos.y;
lengthP = vecLength(&P);
/* Move the ball out of wall */
while (lengthP > sqrt ((radius - inBall->ballRadius) * (radius - inBall->ballRadius)))
{
counter++;
inBall->pos.x += inBall->ballVel.x * 0.05;
inBall->pos.y += inBall->ballVel.y * 0.05;
P.x = cx - inBall->pos.x;
P.y = cy - inBall->pos.y;
lengthP = vecLength(&P);
// Prevent infinite loops
if (counter == 30)
break;
}
}
}
void AddrMem::convertType(ostream& os) const
{
os << "(";
os << PrecType::getName(prec());
if (vecLength() > 1) os << vecLength();
os << ")";
}
void AddrMem::declareType(ostream& os) const
{
os << _addrSpace.str();
if (_isConst) os << "const ";
os << PrecType::getName(prec());
if (vecLength() > 1) os << vecLength();
os << (_isPointer ? " * " : " ");
}
void Particle::update(const std::vector< std::vector<sf::Vector2f> > &map) {
// TODO: simplify, make faster
if (vvel == sf::Vector2f(0, 0) || vecLength(vvel) < .00001f) {
return;
}
sf::Time time_now = clock->getElapsedTime();
if (lastUpdated.asMicroseconds() == 0) {
// El primer frame del joc ningú es mou
lastUpdated = time_now;
return;
}
long long micros = time_now.asMicroseconds() - lastUpdated.asMicroseconds();
sf::Vector2f a = vecUnit(vvel) * float(accel * micros);
if (vecLength(a) < float(accel * micros)) {
vvel = sf::Vector2f(0, 0);
accel = 0;
if (disappear) {
alive = false;
}
return;
} else {
vvel += a;
}
target_movement = vvel * float(micros);
lastUpdated = clock->getElapsedTime();
prev_pos = position;
position += target_movement;
if (!between(position.x, -20000, 20000) || !between(position.y, -20000, 20000)) {
alive = false;
return;
}
if (collide) {
for (auto &polygon : map) {
// TODO: Try to call this less
if (lineCrossesPoly(prev_pos, position, polygon)) {
vvel = sf::Vector2f(0, 0);
if (disappear) {
alive = false;
}
return;
}
}
}
}
/* virtual */ SysStatusUval
FileLinuxDevZero::writev(const struct iovec *vec, uval vecCount,
ThreadWait **tw, GenState &moreAvail)
{
moreAvail.state = FileLinux::READ_AVAIL | FileLinux::WRITE_AVAIL;
return _SRETUVAL(vecLength(vec, vecCount));
}
/**
* Adds a point for inside the sphere.
* @param v point
*/
void add(const Vec3f &v)
{
const float vMinusCenterLen = vecLength(v - m_center);
if ( vMinusCenterLen > m_radius )
m_radius = vMinusCenterLen;
}
/* virtual */ SysStatusUval
FileLinuxFile::writev(const struct iovec *vec, uval vecCount,
ThreadWait **tw, GenState &moreAvail)
{
FLFDEBUG("writev");
SysStatusUval rc;
uval length_write;
char* buf_write;
uval length = vecLength(vec, vecCount);
lock();
moreAvail.state = FileLinux::READ_AVAIL|FileLinux::WRITE_AVAIL;
rc = locked_writeAlloc(length, buf_write, tw);
if (_FAILURE(rc)) {
unLock();
return rc;
}
length_write = _SGETUVAL(rc);
if (length_write) {
memcpy_fromiovec(buf_write, vec, vecCount, length_write);
locked_writeFree(buf_write);
}
unLock();
return rc;
}
/* virtual */ SysStatusUval
FileLinuxFile::readv(struct iovec* vec, uval vecCount,
ThreadWait **tw, GenState &moreAvail)
{
FLFDEBUG("readv");
SysStatusUval rc;
uval length_read;
char* buf_read;
uval length = vecLength(vec,vecCount);
lock();
moreAvail.state = FileLinux::READ_AVAIL|FileLinux::WRITE_AVAIL;
rc = locked_readAlloc(length, buf_read, tw);
if (_FAILURE(rc)) {
unLock();
//semantics shift from EOF error to 0 length on EOF
if (_SCLSCD(rc) == FileLinux::EndOfFile) {
moreAvail.state = FileLinux::ENDOFFILE;
return 0;
}
return rc;
}
length_read = _SGETUVAL(rc);
if (length_read) {
memcpy_toiovec(vec, buf_read, vecCount, length_read);
locked_readFree(buf_read);
}
unLock();
return rc;
}
void vecNormalize(Vec v)
{
Flt len = vecLength(v);
if (len == 0.0) {
vecMake(0,0,1,v);
return;
}
len = 1.0 / len;
v[0] *= len;
v[1] *= len;
v[2] *= len;
}
void updateAppendage(Scene* scene, int sceneIndex, cl_float3 p, cl_float3 q,
cl_float3 w, float A, float B, int countA, int countB) {
cl_float3 U;
cl_float3 V;
cl_float3 W;
cl_float3 j;
cl_float3 d;
V = q;
vecSub(V, p);
float D = vecLength(V);
float inverseD = 1.0f / D;
vecScale(V, inverseD);
W = w;
vecNormalize(W);
vecCross(U, V, W);
float A2 = A*A;
float y = 0.5f * inverseD * (A2 - B * B + D * D);
double square = A2 - y*y;
if (square < 0.0f)
throw std::runtime_error("Unable to construct appendage");
float x = sqrtf(square);
j = p;
vecScaledAdd(j, x, U);
vecScaledAdd(j, y, V);
d = j;
vecSub(d, p);
vecScale(d, 1.0f / float(countA));
for (int i = 0; i <= countA; i++) {
Sphere* sphere = &(scene->spheres[sceneIndex + i]);
sphere->center = p;
vecScaledAdd(sphere->center, float(i), d);
vecScale(sphere->center, 0.1f);
}
d = j;
vecSub(d, q);
vecScale(d, 1.0f / float(countB));
for (int i = 0; i <= countA; i++) {
Sphere* sphere = &(scene->spheres[sceneIndex + i + countA + 1]);
sphere->center = q;
vecScaledAdd(sphere->center, float(i), d);
vecScale(sphere->center, 0.1f);
}
}
/**
* Adds a sphere.
*/
void add(const BoundingSphere &sphere)
{
const float sCenterMinusCenterLen = vecLength(sphere.center() - m_center);
if ( sCenterMinusCenterLen > m_radius )
{
m_center = (sphere.center() + m_center)/2.0f;
m_radius = sCenterMinusCenterLen + hgMax(sphere.radius(), m_radius);
}
else if ( m_radius < sphere.radius() )
{
*this = sphere;
}
}
void Line::alignWithVec(Matrix<double> vec)
{
Matrix<double> new_vec = vecNormalize(vec)*vecLength(p_position_b-p_position_a);
Matrix<double> middle(3, 1);
middle[0] = p_position_a[0] + 0.5 * (p_position_b[0] - p_position_a[0]);
middle[1] = p_position_a[1] + 0.5 * (p_position_b[1] - p_position_a[1]);
middle[2] = p_position_a[2] + 0.5 * (p_position_b[2] - p_position_a[2]);
p_position_a = middle - 0.5*new_vec;
p_position_b = middle + 0.5*new_vec;
computePrism();
computeVertices();
}
void setValues(fwPlane& _plane, const fwVec3d & _point1, const fwVec3d & _point2, const fwVec3d & _point3)
{
SLM_TRACE_FUNC();
fwVec3d normalVec= cross(_point2 - _point1, _point3 -_point1);
if((float)(vecLength(normalVec)) <= 0.0F) {
normalVec[0] =0.0F;
normalVec[1] =0.0F;
normalVec[2] =1.0F;
}
normalize(normalVec);
// Normal
setNormal(_plane, normalVec);
// Distance
double distance = normalVec[0]*_point1[0] + normalVec[1]*_point1[1] + normalVec[2]*_point1[2];
setDistance(_plane, distance);
}
/* virtual */ SysStatusUval
FileLinuxVirtFile::readv(struct iovec* vec, uval vecCount,
ThreadWait **tw, GenState &moreAvail)
{
/* FIXME: for now implementing only for vecCount == 1.
* In this case it's not necessary to allocate a buffer for
* performing the read operation and then invoke memcpy_toiovec
*/
tassertMsg(vecCount == 1, "FileLinuxVirtFile:readv does not support "
"vecCount > 1 yet. See FIXME note.\n");
SysStatusUval rc;
uval length_read = 0, l, lr;
uval length = vecLength(vec,vecCount);
streamLock.acquire();
moreAvail.state = FileLinux::READ_AVAIL|FileLinux::WRITE_AVAIL;
char *buf_read = (char*) vec->iov_base;
char *buf_cur_ptr = buf_read;
while (length > 0) {
l = length;
if (l > MAX_IO_LOAD) {
l = MAX_IO_LOAD;
}
rc = stub._read(buf_cur_ptr, l);
if (_FAILURE(rc)) {
//semantics shift from EOF error to 0 length on EOF
if (_SCLSCD(rc) == FileLinux::EndOfFile) {
moreAvail.state = FileLinux::ENDOFFILE;
rc = 0;
}
streamLock.release();
return rc;
}
lr = _SGETUVAL(rc);
length_read += lr;
buf_cur_ptr += lr;
length -= lr;
if (lr < l) break; // got all there is to it
}
streamLock.release();
return _SRETUVAL(length_read);
}
/* virtual */ SysStatusUval
FileLinuxVirtFile::writev(const struct iovec *vec, uval vecCount,
ThreadWait **tw, GenState &moreAvail)
{
/* FIXME: for now implementing only for vecCount == 1.
* In this case it's not necessary to allocate a buffer for
* performing the read operation and then invoke memcpy_toiovec
*/
tassertMsg(vecCount == 1, "FileLinuxVirtFile:writev does not support "
"vecCount > 1 yet. See FIXME note.\n");
SysStatusUval rc;
uval length_written = 0, l, lw;
char* buf_write = (char*) vec->iov_base;
uval length = vecLength(vec, vecCount);
streamLock.acquire();
moreAvail.state = FileLinux::READ_AVAIL|FileLinux::WRITE_AVAIL;
char *buf_cur_ptr = buf_write;
while (length > 0) {
l = length;
if (l > MAX_IO_LOAD) {
l = MAX_IO_LOAD;
}
rc = stub._write(buf_cur_ptr, l);
if (_FAILURE(rc)) {
streamLock.release();
return rc;
}
lw = _SGETUVAL(rc);
length_written += lw;
buf_cur_ptr += lw;
length -= lw;
if (lw < l) break; // couldn't write all requested
}
streamLock.release();
return _SRETUVAL(length_written);
}
bool sphereSphereCollision(const BoundingSphere &M,
const Vec3f &vel,
const BoundingSphere &S)
{
const Vec3f e = M.center() - S.center();
const float r = S.radius() + M.radius();
if ( vecLength(e) < r )
return true;
const float delta = Math::square(vecDot(e, vel))
- vecDot(vel, vel) * (vecDot(e, e) - r*r);
if ( delta < 0.0f )
return false;
const float t = (-vecDot(e, vel) - std::sqrt(delta)) / vecDot(vel, vel);
if ( t < 0.0f || t > 1.0f )
return false;
return true;
}
/* virtual */ SysStatusUval
StreamServerConsole::sendto(struct iovec* vec, uval veclen, uval flags,
const char *addr, uval addrLen,
GenState &moreAvail,
void *controlData, uval controlDataLen,
__XHANDLE xhandle)
{
uval len = vecLength(vec, veclen);
char* buf = (char*)allocLocalStrict(len);
tassertMsg((controlDataLen == 0), "oops\n");
memcpy_fromiovec(buf, vec, veclen, len);
SysConsole->write(buf, len);
freeLocalStrict(buf, len);
calcAvailable(moreAvail);
if (XHANDLE_IDX(xhandle) != CObjGlobalsKern::ConsoleIndex) {
clnt(xhandle)->setAvail(moreAvail);
}
return len;
}
size_t Gathering::gatherSubscript(const size_t variableNumber,
const size_t N,
const bool xHasIndex,
const bool yHasIndex,
const size_t xOffset,
const size_t yOffset)
{
const AppSubscript appSub(N, xHasIndex, yHasIndex, xOffset, yOffset);
if ( _appSubscript[variableNumber].count(appSub) )
{
return _appSubscript[ variableNumber ][ appSub ];
}
const size_t effVecLen = vecLength(variableNumber);
const ArraySubscript arrSub(variableNumber,
N,
xHasIndex,
xOffset / effVecLen,
yHasIndex,
yOffset);
if ( _subscriptToNumber.count(arrSub) )
{
return _appSubscript[ variableNumber ][ appSub ]
= _subscriptToNumber[ arrSub ];
}
_numberToSubscript[ _subscriptCounter ] = arrSub;
_subscriptToNumber[ arrSub ] = _subscriptCounter;
_appSubscript[ variableNumber ][ appSub ] = _subscriptCounter;
return _subscriptCounter++;
}
// vecNormalize - Returns a unit vector in the same direction.
// Result can be the same vector as the vector argument.
void vecNormalize(geomVec v, geomVec result)
{
double norm = vecLength(v);
result[0] = v[0]/norm;
result[1] = v[1]/norm;
}
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;
}
}
}
inline Vec4f vecNormal(const Vec4f &v) {
const float len = 1.0f/vecLength(v);
return Vec4f(v[0] * len, v[1] * len, v[2] * len, v[3] * len);
}
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;
}
// Virtual functions
void Vector2Test::Run( std::ostream & p_Trace )
{
std::cout << "-------------------------------------------" << std::endl;
std::cout << "Starting Vector2 test." << std::endl;
// Run a constuctor test for the different types
Bit::Vector2i8 veci8( 1, 2 );
Bit::Vector2u8 vecu8( 1, 2 );
Bit::Vector2i16 veci16( 1, 2 );
Bit::Vector2u16 vecu16( 1, 2 );
Bit::Vector2i32 veci32( 1, 2 );
Bit::Vector2u32 vecu32( 1, 2 );
Bit::Vector2i64 veci64( 1, 2 );
Bit::Vector2u64 vecu64( 1, 2 );
Bit::Vector2f32 vecf32( 1.0f, 2.0f );
Bit::Vector2f64 vecf64( 1.0f, 2.0f );
// Assert the vectors
TestAssert( veci8.x == 1 && veci8.y == 2 );
TestAssert( vecu8.x == 1 && vecu8.y == 2 );
TestAssert( veci16.x == 1 && veci16.y == 2 );
TestAssert( vecu16.x == 1 && vecu16.y == 2 );
TestAssert( veci32.x == 1 && veci32.y == 2 );
TestAssert( vecu32.x == 1 && vecu32.y == 2 );
TestAssert( veci64.x == 1 && veci64.y == 2 );
TestAssert( vecu64.x == 1 && vecu64.y == 2 );
TestAssert( vecf32.x == 1.0f && vecf32.y == 2.0f );
TestAssert( vecf64.x == 1.0f && vecf64.y == 2.0f );
// Casting tests
Bit::Vector2f64 vecCast1( vecf32 );
Bit::Vector2f32 vecCast2( vecf64 );
TestAssert( vecCast1.x == vecf32.x && vecCast1.y == vecf32.y);
TestAssert( vecCast2.x == vecf64.x && vecCast2.y == vecf64.y);
// Assert Magnitude function
Bit::Vector2f32 vecLength( 100.0f, 0.0f );
TestAssert( vecLength.Length( ) == 100.0f );
// Assert normal function
Bit::Vector2f32 vecNormal1 = Bit::Vector2f32( 123.0f, 0.0f ).Normal( );
TestAssert( vecNormal1.x == 1.0f && vecNormal1.y == 0.0f );
// Assert normalize function
Bit::Vector2f32 vecNormal2( 123.0f, 0.0f );
vecNormal2.Normalize( );
TestAssert( vecNormal2.x == 1.0f && vecNormal2.y == 0.0f );
// Assert dot product function
TestAssert( Bit::Vector2f32( -1.0f, 0.0f ).Dot( Bit::Vector2f32( 1.0f, 0.0f ) ) == -1.0f );
TestAssert( Bit::Vector2f32( 0.0f, 1.0f ).Dot( Bit::Vector2f32( 1.0f, 0.0f ) ) == 0.0f );
TestAssert( Bit::Vector2f32( 1.0f, 0.0f ).Dot( Bit::Vector2f32( 1.0f, 0.0f ) ) == 1.0f );
// Assert angle between vectors function
Bit::Vector2f32 vecAngle1( -1.0f, 0.0f );
Bit::Vector2f32 vecAngle2( 0.0f, 1.0f );
TestAssert( Bit::Math::EqualEpsilonHalf<Bit::Float64>( Bit::Vector2f32::AngleBetweenVectors( vecAngle1, vecAngle2 ).AsDegrees( ), 90.0f ) );
// Assert the absolute function
Bit::Vector2f32 vecAbs1 = Bit::Vector2f32( -1.0f, -2.0f ).Absolute( );
Bit::Vector2f32 vecAbs2 = Bit::Vector2f32( 1.0f, -2.0f ).Absolute( );
Bit::Vector2f32 vecAbs3 = Bit::Vector2f32( -1.0f, 2.0f ).Absolute( );
Bit::Vector2f32 vecAbs4 = Bit::Vector2f32( 1.0f, 2.0f ).Absolute( );
TestAssert( vecAbs1.x == 1.0f && vecAbs1.y == 2.0f );
TestAssert( vecAbs2.x == 1.0f && vecAbs2.y == 2.0f );
TestAssert( vecAbs3.x == 1.0f && vecAbs3.y == 2.0f );
TestAssert( vecAbs4.x == 1.0f && vecAbs4.y == 2.0f );
// Assert the rotate function
Bit::Vector2f32 vecRotate( 0.0f, 1.0f );
vecRotate.Rotate( Bit::Degrees( 90.0f ) );
TestAssert( Bit::Math::EqualEpsilonHalf<Bit::Float64>( vecRotate.x, -1.0f ) &&
Bit::Math::EqualEpsilonHalf<Bit::Float64>( vecRotate.y, 0.0f ) );
// Print the finish text
std::cout << "Finished Vector2 Test." << std::endl;
std::cout << "-------------------------------------------" << std::endl;
}
/**
* Adds a sphere.
*/
void Add(const BoundingSphere &sphere) {
const float centers_dist = vecLength(m_center - sphere.get_center());
m_center = (sphere.get_center() + m_center)*0.5f;
m_radius = centers_dist*0.5f + std::max(m_radius, sphere.get_radius());
}
double Line::length() const
{
return vecLength(p_position_a - p_position_b) + p_offset_a + p_offset_b;
}
