// ****** Rotation Matrix from Two Vector Directions ********
// ****** given two vector directions (v1 and v2) known in two frames (b and e) find Rbe ***
// ****** solution is approximate if can't be exact ***
uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[3], const float v2e[3], float Rbe[3][3])
{
float Rib[3][3], Rie[3][3];
float mag;
uint8_t i,j,k;
// identity rotation in case of error
for (i=0;i<3;i++){
for (j=0;j<3;j++)
Rbe[i][j]=0;
Rbe[i][i]=1;
}
// The first rows of rot matrices chosen in direction of v1
mag = VectorMagnitude(v1b);
if (fabs(mag) < 1e-30)
return (-1);
for (i=0;i<3;i++)
Rib[0][i]=v1b[i]/mag;
mag = VectorMagnitude(v1e);
if (fabs(mag) < 1e-30)
return (-1);
for (i=0;i<3;i++)
Rie[0][i]=v1e[i]/mag;
// The second rows of rot matrices chosen in direction of v1xv2
CrossProduct(v1b,v2b,&Rib[1][0]);
mag = VectorMagnitude(&Rib[1][0]);
if (fabs(mag) < 1e-30)
return (-1);
for (i=0;i<3;i++)
Rib[1][i]=Rib[1][i]/mag;
CrossProduct(v1e,v2e,&Rie[1][0]);
mag = VectorMagnitude(&Rie[1][0]);
if (fabs(mag) < 1e-30)
return (-1);
for (i=0;i<3;i++)
Rie[1][i]=Rie[1][i]/mag;
// The third rows of rot matrices are XxY (Row1xRow2)
CrossProduct(&Rib[0][0],&Rib[1][0],&Rib[2][0]);
CrossProduct(&Rie[0][0],&Rie[1][0],&Rie[2][0]);
// Rbe = Rbi*Rie = Rib'*Rie
for (i=0;i<3;i++)
for(j=0;j<3;j++){
Rbe[i][j]=0;
for(k=0;k<3;k++)
Rbe[i][j] += Rib[k][i]*Rie[k][j];
}
return 1;
}
float Angle(vector3d &v1, vector3d &v2)
{
float Dp1 = VectorMagnitude(v1)*VectorMagnitude(v2); // missing the angle calc
float Dp2 = (v1.x*v2.x) + (v1.y*v2.y) + (v1.z*v2.z);
float Ang = Dp2 / Dp1;
if (Ang > 1.00000f)
Ang = 1.00000f;
if (Ang < -1.00000f)
Ang = -1.00000f;
Ang = (float)acos(Ang); //inverse cosine ang (cos-1 aka arccos)
return Ang;
}
/*
* From Roll-Pitch Gyro Drift Compensation, Rev 3. William Premerlani, 2012.
*/
static void rollPitch_drift_GPS(float Rbe[3][3], float accels_e_int[3],
float delT_between_updates, float *errRollPitch_b)
{
float errRollPitch_e[3];
float dGPSdt_e[3];
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
dGPSdt_e[0] = (gpsVelocity.North - drft->GPSV_old[0]) / delT_between_updates;
dGPSdt_e[1] = (gpsVelocity.East - drft->GPSV_old[1]) / delT_between_updates;
dGPSdt_e[2] = -GRAVITY + (gpsVelocity.Down - drft->GPSV_old[2]) / delT_between_updates;
drft->GPSV_old[0] = gpsVelocity.North;
drft->GPSV_old[1] = gpsVelocity.East;
drft->GPSV_old[2] = gpsVelocity.Down;
float normdGPSdt_e = VectorMagnitude(dGPSdt_e);
//Take cross product of integrated accelerometer measurements with integrated earth frame accelerations. We should be using normalized dGPSdt, but we perform that calculation in the following line(s).
CrossProduct((const float *)accels_e_int, (const float *)dGPSdt_e, errRollPitch_e);
//Scale cross product
errRollPitch_e[0] /= (normdGPSdt_e * delT_between_updates);
errRollPitch_e[1] /= (normdGPSdt_e * delT_between_updates);
errRollPitch_e[2] /= (normdGPSdt_e * delT_between_updates);
//Rotate earth drift error back into body frame;
rot_mult(Rbe, errRollPitch_e, errRollPitch_b, FALSE);
}
/* define behavior for bots in the attack state */
void AIAttackEnter(ai_t *brain){
player_t *enemy;
brain->prevState = brain->curState;
brain->timer = 0;
if((enemy = AIGetEnemy()) != NULL){
/* get a vector pointing towards the enemy */
VectorSubtract(&brain->enemyDir, &enemy->position, &brain->body->position);
/* get the distance to the enemy */
brain->enemy_dis = VectorMagnitude(&brain->enemyDir);
VectorScale(&brain->enemyDir, &brain->enemyDir, 1.0/brain->enemy_dis);
/* decide if we need to move closer and if we do, we're in the wrong
* state (should be in reposition */
if(brain->enemy_dis > AI_DESIRED_RANGE){
AIRepositionEnter(brain);
}
else{
brain->curState = AI_ATTACK;
brain->timeout = AI_ATTACK_TIMEOUT + R_TIME_MOD*rand();
PlayerSetAnimation(brain->body, PLAYER_ANIM_IDLE2);
PlayerReset(brain->body);
}
}
}
void Rv2Rot(float Rv[3], float R[3][3])
{
// Compute rotation matrix from a rotation vector
// To save .text space, uses Quaternion2R()
float q[4];
float angle = VectorMagnitude(Rv);
if (angle <= 0.00048828125f) {
// angle < sqrt(2*machine_epsilon(float)), so flush cos(x) to 1.0f
q[0] = 1.0f;
// and flush sin(x/2)/x to 0.5
q[1] = 0.5f*Rv[0];
q[2] = 0.5f*Rv[1];
q[3] = 0.5f*Rv[2];
// This prevents division by zero, while retaining full accuracy
}
else {
q[0] = cosf(angle*0.5f);
float scale = sinf(angle*0.5f) / angle;
q[1] = scale*Rv[0];
q[2] = scale*Rv[1];
q[3] = scale*Rv[2];
}
Quaternion2R(q, R);
}
CVector3 VectorNormalize(CVector3 vVector)
{
float magnitude = VectorMagnitude(vVector);
vVector = vVector / magnitude;
return vVector;
}
/* angle between vectors - rad version */
static inline D3DVALUE AngleBetweenVectorsRad (const D3DVECTOR *a, const D3DVECTOR *b)
{
D3DVALUE la, lb, product, angle, cos;
/* definition of scalar product: a*b = |a|*|b|*cos... therefore: */
product = ScalarProduct (a,b);
la = VectorMagnitude (a);
lb = VectorMagnitude (b);
if (!la || !lb)
return 0;
cos = product/(la*lb);
angle = acos(cos);
TRACE("angle between (%f,%f,%f) and (%f,%f,%f) = %f radians (%f degrees)\n", a->x, a->y, a->z, b->x,
b->y, b->z, angle, RadToDeg(angle));
return angle;
}
// Unit vector in the v direction
Vector3D VectorNormalize(Vector3D v)
{
float length = VectorMagnitude(v);
if (length == 0) return v;
return VectorScalarMult( v, 1.0/length);
}
/* calculates the length of vector's projection on another vector */
static inline D3DVALUE ProjectVector (const D3DVECTOR *a, const D3DVECTOR *p)
{
D3DVALUE prod, result;
prod = ScalarProduct(a, p);
result = prod/VectorMagnitude(p);
TRACE("length projection of (%f,%f,%f) on (%f,%f,%f) = %f\n", a->x, a->y, a->z, p->x,
p->y, p->z, result);
return result;
}
void SurfaceNormal(struct coords *NP, struct coords *FP, struct coords *LP)
{
NP->x = FP->y * LP->z - FP->z * LP->y;
NP->y = FP->z * LP->x - FP->x * LP->z;
NP->z = FP->x * LP->y - FP->y * LP->x;
VectorMagnitude(NP);
UnitVector(NP);
} /* SurfaceNormal() */
void NormalizeVector(const GzCoord a, GzCoord &b)
{
float mag = VectorMagnitude(a);
// prevent divide-by-zero
if (mag == 0.0f)
mag = 1.0f;
b[0] = a[0] / mag;
b[1] = a[1] / mag;
b[2] = a[2] / mag;
}
void RotatePoint(struct coords *A, Matx3x3 M)
{
struct coords B;
B.x = A->x * M[0][0] + A->y * M[0][1] + A->z * M[0][2];
B.y = A->x * M[1][0] + A->y * M[1][1] + A->z * M[1][2];
B.z = A->x * M[2][0] + A->y * M[2][1] + A->z * M[2][2];
A->x = B.x;
A->y = B.y;
A->z = B.z;
VectorMagnitude(A);
} /* RotatePoint() */
/* also calculates average heading and position as well as confidence level */
void AIFlockSenseMates(aiflock_t *flock){
elist_t *ptr1, *ptr2;
float tempDis;
vector_t temp;
vector_t heading = {0, 0, 0};
vector_t position = {0, 0, 0};
/* for each flock member */
for(ptr1 = flock->bots->next; ptr1 != flock->bots; ptr1 = ptr1->next){
/* start with no confidence */
ptr1->bot->confidence = 0;
/* for each other flock member besides yourself */
ptr1->bot->mate_dis = 1000000; /* just something big as an initializer */
for(ptr2 = flock->bots->next; ptr2 != flock->bots; ptr2 = ptr2->next){
if(ptr1 != ptr2){
VectorSubtract(&temp, &ptr1->bot->body->position,
&ptr2->bot->body->position);
tempDis = VectorMagnitude2(&temp);
if(tempDis < ptr1->bot->mate_dis){
ptr1->bot->mate_dis = tempDis;
ptr1->bot->mate = ptr2->bot;
}
if(tempDis < AI_CONFIDENCE_MAX){
ptr1->bot->confidence++;
}
} /* end check to make sure you're not looking at yourself */
} /* end loop to look for others besides yourself */
VectorAdd(&heading, &heading, &ptr1->bot->body->forward);
VectorAdd(&position, &position, &ptr1->bot->body->position);
} /* end loop for each flock member */
/* finish average heading and position */
VectorScale(&flock->heading, &heading, 1/flock->nBots);
VectorScale(&flock->position, &position, 1/flock->nBots);
flock->speed = VectorMagnitude(&flock->heading);
VectorScale(&flock->heading, &flock->heading, 1.0/flock->speed);
}
/* define behavior for bots in reposition state */
void AIRepositionEnter(ai_t *brain){
player_t *enemy;
vector_t orthog;
vector_t temp;
float diff;
brain->prevState = brain->curState;
brain->curState = AI_REPOSITION;
brain->timer = 0;
brain->timeout = AI_REPO_TIMEOUT + R_TIME_MOD*rand();
PlayerSetAnimation(brain->body, PLAYER_ANIM_RUN);
PlayerReset(brain->body);
VectorClear(&brain->destination);
/* we're currently fighting, we just want to move slightly
* before attacking again */
if((enemy = AIGetEnemy()) != NULL){
/* get a vector pointing towards the enemy */
VectorSubtract(&brain->enemyDir, &enemy->position, &brain->body->position);
/* get the distance to the enemy */
brain->enemy_dis = VectorMagnitude(&brain->enemyDir);
VectorScale(&brain->enemyDir, &brain->enemyDir, 1.0/brain->enemy_dis);
/* get a vector orthogonal to the direction of the enemy */
VectorCross(&orthog, &brain->enemyDir, &brain->body->up);
/* decide if we need to move closer */
if(brain->enemy_dis > AI_DESIRED_RANGE){
diff = brain->enemy_dis - AI_DESIRED_RANGE + UNIRAND(6.0);
VectorScale(&temp, &brain->enemyDir, diff);
VectorAdd(&brain->destination, &temp, &brain->body->position);
}
/* now move sideways some */
diff = AI_MOVE_SIZE + UNIRAND(6.0);
VectorScale(&temp, &orthog, 0.2*SIGN(UNIRAND(1.0))*diff);
VectorAdd(&brain->destination, &brain->destination, &temp);
}
}
void Laser::Update(float a_Dt)
{
m_Sprite.sprite->setPosition(m_StartPos);
//raycast to determine the max length of this laser
cpVect cpStartPos;
cpStartPos.x = m_StartPos.x;
cpStartPos.y = m_StartPos.y;
sf::Vector2f addVector = m_FacingDir * MAX_LASER_DIST;
sf::Vector2f sfEndPos = m_StartPos + addVector;
cpVect cpEndPos;
cpEndPos.x = sfEndPos.x;
cpEndPos.y = sfEndPos.y;
cpSegmentQueryInfo info;
info.n = cpv(0,0);
info.t = 0;
info.shape = NULL;
//cast ray
cpShape* collided = cpSpaceSegmentQueryFirst(&m_Space, cpStartPos, cpEndPos, ~(cpLayers)COLLIDABLE::GLASSBLOCK, CP_NO_GROUP, &info);
//grab some helper data
m_MaxLength = float(min( cpSegmentQueryHitDist(cpStartPos, cpEndPos, info), MAX_LASER_DIST ));
m_hitPoint = cpSegmentQueryHitPoint(cpStartPos, cpEndPos, info);
//extend or reduce the laser to the max length
if(m_CurLength < m_MaxLength)
{
//update end position
//f**k you, lasers have velocity (but only when they're growing)
sf::Vector2f newDist = m_FacingDir * LASER_VELOCITY * a_Dt;
m_EndPos += newDist;
m_CurLength += VectorMagnitude(newDist);
//update sprite
float baseSize = float(m_Sprite.sprite->getTexture()->getSize().x);
sf::Vector2f diff = m_EndPos - m_StartPos;
float newSize = VectorMagnitude(diff);
m_Sprite.sprite->setScale(newSize / baseSize, 1);
//m_resMgr.RemoveDrawableSprite(&m_endSprite);
}
else
{
//update end position
m_EndPos = m_StartPos + m_FacingDir * m_MaxLength;
sf::Vector2f diff = m_EndPos - m_StartPos;
m_endSprite.sprite->setPosition(m_EndPos);
//sf::Vector2f diff = m_blockPos - m_StartPos;
m_CurLength = VectorMagnitude(diff);
//update sprite
float baseSize = float(m_Sprite.sprite->getTexture()->getSize().x);
m_Sprite.sprite->setScale(m_CurLength / baseSize, 1);
//m_resMgr.AddDrawableSprite(&m_endSprite);
m_endDrawnHack = true;
}
/*if(m_pNextChainSegment)
{
m_pNextChainSegment->Update(a_Dt);
}*/
}
void Laser::Update(float a_Dt)
{
//raycast to determine the max length of this laser
cpVect cpStartPos;
cpStartPos.x = m_StartPos.x;
cpStartPos.y = m_StartPos.y;
sf::Vector2f addVector = m_FacingDir * MAX_LASER_DIST;
sf::Vector2f sfEndPos = m_StartPos + addVector;
cpVect cpEndPos;
cpEndPos.x = sfEndPos.x;
cpEndPos.y = sfEndPos.y;
cpSegmentQueryInfo info;
info.n = cpv(0,0);
info.t = 0;
info.shape = NULL;
//cast ray
cpShape* collided = cpSpaceSegmentQueryFirst(&m_Space, cpStartPos, cpEndPos, CP_ALL_LAYERS, CP_NO_GROUP, &info);
//grab some helper data
m_MaxLength = float(min( cpSegmentQueryHitDist(cpStartPos, cpEndPos, info), MAX_LASER_DIST ));
m_hitPoint = cpSegmentQueryHitPoint(cpStartPos, cpEndPos, info);
GameObject* pGameObj = NULL;
if(collided)
pGameObj = (GameObject*)collided->data;
/*
//check to see if we hit the player
Player* pPlayer = NULL;
bool colPlayer = false;
if(pGameObj && pGameObj->GetType() == PLAYER)
{
//TODO: only reflect if the player is within 180 degrees of us?
//reflect le laser
if(!m_pNextChainSegment)
ExtendNewSegment(m_FacingDir);
pPlayer = (Player*)pGameObj;
m_MaxLength = pPlayer->GetPosition().x;
colPlayer = true;
}
else
{
//stop reflecting laser
Laser* pCurLaser = m_pNextChainSegment;
m_pNextChainSegment = NULL;
while(pCurLaser)
{
m_resMgr.RemoveDrawableSprite(pCurLaser->GetSprite());
pCurLaser = pCurLaser->GetNextSegment();
delete pCurLaser;
}
}*/
//extend or reduce the laser to the max length
if(m_CurLength < m_MaxLength)
{
//update end position
//f**k you, lasers have velocity (but only when they're growing)
sf::Vector2f newDist = m_FacingDir * LASER_VELOCITY * a_Dt;
m_EndPos += newDist;
m_CurLength += VectorMagnitude(newDist);
//update sprite
float baseSize = float(m_Sprite.sprite->getTexture()->getSize().x);
sf::Vector2f diff = m_EndPos - m_StartPos;
float newSize = VectorMagnitude(diff);
m_Sprite.sprite->setScale(newSize / baseSize, 1);
if(m_endDrawn)
{
m_resMgr.RemoveDrawableSprite(&m_endSprite);
m_resMgr.RemoveDrawableSprite(&m_reflectSprite);
m_endDrawn = false;
}
}
else if(m_CurLength > m_MaxLength)
{
bool colPlayer = false;
//update end position
m_EndPos = m_StartPos + m_FacingDir * m_MaxLength;
sf::Vector2f diff = m_EndPos - m_StartPos;
m_CurLength = VectorMagnitude(diff);
//update sprite
float baseSize = float(m_Sprite.sprite->getTexture()->getSize().x);
m_Sprite.sprite->setScale(m_CurLength / baseSize, 1);
//Show the end sprite
if (!m_endDrawn)
{
if(colPlayer)
m_resMgr.AddDrawableSprite(&m_reflectSprite);
else
m_resMgr.AddDrawableSprite(&m_endSprite);
m_endDrawn = true;
}
m_endDrawnHack = true;
}
/*
//update reflected laser segment
if(pPlayer && m_pNextChainSegment)
{
//get the player's angle and position
m_pNextChainSegment->SetPosition(pPlayer->GetPosition());
m_pNextChainSegment->SetFacingDir(pPlayer->GetRedirectDir()); //assumes already normalised
}
//update endsprite
if(m_endDrawn)
{
//m_endSprite.sprite->setPosition(m_Sprite.sprite->getPosition().x+m_Sprite.sprite->getScale().x,m_Sprite.sprite->getPosition().y);
if(colPlayer && pPlayer)
{
sf::Vector2u sprSize = pPlayer->GetSprite()->sprite->getTexture()->getSize();//sprSize
sf::Vector2f playerPos = pPlayer->GetPosition();
m_reflectSprite.sprite->setPosition(playerPos.x, playerPos.y);
}
else
m_endSprite.sprite->setPosition(float(m_hitPoint.x)-4, float(m_hitPoint.y)-16);
if(m_pAnimator)
m_pAnimator->Update(a_Dt);
}
*/
}
void AIAttackUpdate(ai_t *brain){
float strength;
float diff;
static int count;
player_t *enemy;
vector_t prediction = {0.0, 0.0, 0.0};
vector_t temp;
projectile_t *proj;
brain->timer++;
/* use info you can see from the player to figure out where he'll be in
* the next step...we're trying to aim with some accuracy. The following
* calculations approximate the exact solution which can be found for
* the collision time of two moving projectiles. The general solution
* only exists under certain conditions. This always exists, and works
* quite well at reasonable range */
enemy = AIGetEnemy();
if(enemy->lFlag){
VectorSubtract(&prediction, &prediction, &enemy->right);
}
else if(enemy->rFlag){
VectorAdd(&prediction, &prediction, &enemy->right);
}
if(enemy->fFlag){
VectorAdd(&prediction, &prediction, &enemy->forward);
}
else if(enemy->bFlag){
VectorSubtract(&prediction, &prediction, &enemy->forward);
}
VectorSubtract(&temp, &enemy->position, &brain->body->position);
VectorAdd(&temp, &temp, &prediction);
VectorScale(&brain->enemyDir, &temp, enemy->speed);
brain->enemy_dis = VectorMagnitude(&brain->enemyDir);
VectorScale(&brain->enemyDir, &brain->enemyDir, 1.0/brain->enemy_dis);
brain->dotEnemy = VectorDot(&brain->enemyDir, &brain->body->forward);
/* decide if we need to move closer */
if(brain->enemy_dis > AI_DESIRED_RANGE){
diff = brain->enemy_dis - AI_DESIRED_RANGE + UNIRAND(3.0);
VectorScale(&temp, &brain->enemyDir, diff);
VectorAdd(&brain->destination, &temp, &brain->body->position);
AIUpdateForwardByDesire(brain, &brain->destination);
PlayerMoveForward(brain->body, brain->body->speed);
}
else{
if(brain->dotEnemy < 1){
strength = 2 - brain->dotEnemy;
/* we need to aim at our opponent pitch/yaw */
brain->body->yaw +=
MAX_TURN*strength*(AIGetTurnDirectionH(brain, &brain->enemyDir));
brain->body->pitch +=
MAX_TURN*strength*(AIGetTurnDirectionV(brain, &brain->enemyDir));
}
if(!(count % AI_ATTACK_PERIOD)){
/* fire the projectile in the exactly direction even though we may
* not have our bodies pointed there yet */
proj = ProjectileNew(&brain->enemyDir, &brain->body->position,
PROJ_SPEED, 2);
PListInsertAfter(plist, proj);
count = 0;
} count++;
}
}
/*
* Although yaw correction is done in horizontal plane, it is
* computed in 3 dimensions, just in case we change our minds later.
*/
void yaw_drift_MagGPS(float Rbe[3][3], bool gpsNewData_flag, bool magNewData_flag, float *errYaw_b)
{
#if defined (PIOS_INCLUDE_GPS) || defined (PIOS_INCLUDE_MAGNETOMETER)
// Form the horizontal direction over ground based on rmat
float horizDirOverGndRmat[3];
//Define forward vector in earth frame, Rbe' * [1;0;0], and eliminate vertical component from vector
horizDirOverGndRmat[0] = Rbe[0][0];
horizDirOverGndRmat[1] = Rbe[0][1];
horizDirOverGndRmat[2] = 0;
#if defined (PIOS_INCLUDE_GPS)
if (gpsNewData_flag) {
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
if (fabs(gpsVelocity.North) > GPS_SPEED_MIN
|| fabs(gpsVelocity.East) > GPS_SPEED_MIN) {
float errorGPSYaw_e;
float errorGPSYaw_b[3];
float normGPS =
sqrtf(gpsVelocity.North * gpsVelocity.North +
gpsVelocity.East * gpsVelocity.East);
float horizDirOverGndGPS[3] = { gpsVelocity.North / normGPS, gpsVelocity.East / normGPS, 0 }; //Normalized vector
// vector cross product to get the rotation error in ground frame. However, save several processor cycles by
// condensing the math to take advantage of the cross products null entries on the x and y elements.
// errorGPSYaw_e = horizDirOverGndRmat X horizDirOverGndGPS
errorGPSYaw_e =
horizDirOverGndRmat[0] * horizDirOverGndGPS[1] -
horizDirOverGndGPS[0] * horizDirOverGndRmat[1];
// Rotate error to body frame. Again, take advantage of the yaw error vector [0;0;errorGPSYaw_e]'s null entries
// errorGPSYaw_b = Rbe * errorGPSYaw_e;
errorGPSYaw_b[0] = Rbe[0][2] * errorGPSYaw_e;
errorGPSYaw_b[1] = Rbe[1][2] * errorGPSYaw_e;
errorGPSYaw_b[2] = Rbe[2][2] * errorGPSYaw_e;
//Sum error with existing error
errYaw_b[0] += errorGPSYaw_b[0] * GPS_YAW_KP;
errYaw_b[1] += errorGPSYaw_b[1] * GPS_YAW_KP;
errYaw_b[2] += errorGPSYaw_b[2] * GPS_YAW_KP;
}
}
#endif
// TODO: Create a flag to indicate whether mag data is present (or check for updates of MagnetometerData)
#if defined (PIOS_INCLUDE_MAGNETOMETER)
if (magNewData_flag) {
MagnetometerData magnetometerData;
MagnetometerGet(&magnetometerData);
float errorMagYaw_e;
float errorMagYaw_b[3];
float horizDirOverGndMag[3];
float mags_b[3] = { magnetometerData.x, magnetometerData.y, magnetometerData.z };
// Rotate magnetometer to earth frame and eliminate vertical component
rot_mult(Rbe, mags_b, horizDirOverGndMag, true);
horizDirOverGndMag[2] = 0;
//Compute norm
float normMag = VectorMagnitude(horizDirOverGndMag);
//Normalize mag_vector. Recall that horizDirOverGndMag[2]=0
horizDirOverGndMag[0] /= normMag;
horizDirOverGndMag[1] /= normMag;
// vector cross product to get the rotation error in ground frame. However, save several processor cycles by
// condensing the math to take advantage of the cross products null entries on the x and y elements.
// errorMagYaw_e = horizDirOverGndRmat X horizDirOverGndMag
errorMagYaw_e = horizDirOverGndRmat[0] * horizDirOverGndMag[1] -
horizDirOverGndMag[0] * horizDirOverGndRmat[1];
// Rotate error to body frame. Again, take advantage of the yaw error vector [0;0;errorGPSYaw_e]'s null entries
// errorMagYaw_b = Rbe * errorMagYaw_e;
errorMagYaw_b[0] = Rbe[0][2] * errorMagYaw_e;
errorMagYaw_b[1] = Rbe[1][2] * errorMagYaw_e;
errorMagYaw_b[2] = Rbe[2][2] * errorMagYaw_e;
errYaw_b[0] += errorMagYaw_b[0] * MAG_YAW_KP;
errYaw_b[1] += errorMagYaw_b[1] * MAG_YAW_KP;
errYaw_b[2] += errorMagYaw_b[2] * MAG_YAW_KP;
}
#endif
#endif
}
/*
* Correct sensor drift, using the DCM approach from W. Premerlani et. al
*/
void Premerlani_GPS(float *accels, float *gyros, float Rbe[3][3], const float delT, bool GPS_Drift_Compensation, GlobalAttitudeVariables *glblAtt, float *omegaCorrP)
{
float errYaw_b[3] = { 0, 0, 0 };
float errRollPitch_b[3] = { 0, 0, 0 };
float normOmegaScalar = VectorMagnitude(gyros);
//Correct roll-pitch drift via GPS and accelerometer
//The math is derived from Roll-Pitch Gyro Drift Compensation, Rev.3, by W. Premerlani
#if defined (PIOS_INCLUDE_GPS)
if (drft->gpsPresent_flag && GPS_Drift_Compensation) {
float accels_e[3];
//Rotate accelerometer readings into Earth frame. Note that we need to take the transpose of Rbe.
rot_mult(Rbe, accels, accels_e, TRUE);
//Integrate accelerometer measurements in Earth frame
drft->accels_e_integrator[0] += accels_e[0] * delT;
drft->accels_e_integrator[1] += accels_e[1] * delT;
drft->accels_e_integrator[2] += accels_e[2] * delT;
drft->delT_between_GPS += delT;
//Check if the GPS has new information.
if (!
(drft->
gpsVelocityDataConsumption_flag & GPS_CONSUMED_BY_RPY)) {
//Compute drift correction, errRollPitch_b, from GPS
rollPitch_drift_GPS(Rbe, drft->accels_e_integrator,
drft->delT_between_GPS,
errRollPitch_b);
//Reset integrator
memset(drft->accels_e_integrator, 0,
sizeof(drft->accels_e_integrator));
//Mark GPS data as consumed by this function
drft->gpsVelocityDataConsumption_flag |=
GPS_CONSUMED_BY_RPY;
drft->delT_between_GPS = 0;
}
}
#endif
if (!GPS_Drift_Compensation) {
#if defined (PIOS_INCLUDE_GPS) && 0 || defined (PIOS_INCLUDE_MAGNETOMETER)
if (!(drft->gpsVelocityDataConsumption_flag & GPS_CONSUMED_BY_Y)) {
// We're actually using new GPS data here, but it's already been stored in old by the previous function
yaw_drift_MagGPS(Rbe, true, drft->magNewData_flag, errYaw_b);
// Mark GPS data as consumed by this function
drft->gpsVelocityDataConsumption_flag |= GPS_CONSUMED_BY_Y;
} else {
// In addition to calculating the roll-pitch-yaw error, we can calculate yaw drift, errYaw_b, based on GPS and attitude data
// We're actually using new GPS data here, but it's already been stored in old by the previous function
yaw_drift_MagGPS(Rbe, false, drft->magNewData_flag, errYaw_b);
}
// Reset flag. Not the best place to do it, but it's messy anywhere else
if (drft->magNewData_flag) {
drft->magNewData_flag = false;
}
#endif
//In addition, we can calculate roll-pitch error with only the aid of an accelerometer
#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
AirspeedActualData airspeedActualData;
AirspeedActualGet(&airspeedActualData);
float airspeed_tas = airspeedActualData.TrueAirspeed;
#else
float airspeed_tas = 0;
#endif
rollPitch_drift_accel(accels, gyros, Rbe, airspeed_tas,
errRollPitch_b);
}
// Calculate gyro drift, based on all errors
gyro_drift(gyros, errYaw_b, errRollPitch_b, normOmegaScalar, delT, omegaCorrP, drft->omegaCorrI);
//Calculate final drift response
gyros[0] += omegaCorrP[0] + drft->omegaCorrI[0];
gyros[1] += omegaCorrP[1] + drft->omegaCorrI[1];
gyros[2] += omegaCorrP[2] + drft->omegaCorrI[2];
//Add 0.0001% of proportional error back into gyroscope bias offset. This keeps DC elements out of the raw gyroscope data.
glblAtt->gyro_correct_int[0] += omegaCorrP[0] / 1000000.0f;
glblAtt->gyro_correct_int[1] += omegaCorrP[1] / 1000000.0f;
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
glblAtt->gyro_correct_int[2] += -gyros[2] * glblAtt->yawBiasRate;
}
/**
* It connects waypoints together with straight lines, and fillets, so that the
* vehicle dynamics, i.e. Dubin's cart constraints, are taken into account. However
* the "direct with filleting" path planner still assumes that there are no obstacles
* along the path.
* The general approach is that before adding a new segment, the
* path planner looks ahead at the next waypoint, and adds in fillets that align the vehicle with
* this next waypoint.
* @param[in] original the flight model to process
* @param[out] new the resulting flight model
* @return true for success, false for failure
*/
bool PathFillet::processPath(FlightDataModel *model)
{
new_model = new FlightDataModel(this);
int newWaypointIdx = 0;
float pos_prev[3];
float pos_current[3];
float pos_next[3];
float previous_curvature;
for(int wpIdx = 0; wpIdx < model->rowCount(); wpIdx++) {
// Get the location
pos_current[0] = model->data(model->index(wpIdx, FlightDataModel::NED_NORTH)).toDouble();
pos_current[1] = model->data(model->index(wpIdx, FlightDataModel::NED_EAST)).toDouble();
pos_current[2] = model->data(model->index(wpIdx, FlightDataModel::NED_DOWN)).toDouble();
// Get the internal parameters
quint8 Mode = model->data(model->index(wpIdx, FlightDataModel::MODE), Qt::UserRole).toInt();
float ModeParameters = model->data(model->index(wpIdx, FlightDataModel::MODE_PARAMS)).toFloat();
float finalVelocity = model->data(model->index(wpIdx, FlightDataModel::VELOCITY)).toFloat();
// Determine if the path is a straight line or if it arcs
float curvature = 0;
switch (Mode)
{
case Waypoint::MODE_CIRCLEPOSITIONRIGHT:
return false;
case Waypoint::MODE_FLYCIRCLERIGHT:
case Waypoint::MODE_DRIVECIRCLERIGHT:
curvature = 1.0f/ModeParameters;
break;
case Waypoint::MODE_CIRCLEPOSITIONLEFT:
return false;
case Waypoint::MODE_FLYCIRCLELEFT:
case Waypoint::MODE_DRIVECIRCLELEFT:
curvature = -1.0f/ModeParameters;
break;
}
// First waypoint cannot be fileting since we don't have start. Keep intact.
if (wpIdx == 0) {
setNewWaypoint(newWaypointIdx++, pos_current, finalVelocity, curvature);
continue;
}
// Only add fillets if the radius is greater than 0, and this is not the last waypoint
if (fillet_radius > 0 && wpIdx < (model->rowCount() - 1))
{
// If waypoints have been set on the new model then use that to know the previous
// location. Otherwise this is setting the first segment. On board that uses the
// current location but while planning offline this is unknown so we use home.
if (newWaypointIdx > 0) {
pos_prev[0] = new_model->data(new_model->index(newWaypointIdx-1, FlightDataModel::NED_NORTH)).toDouble();
pos_prev[1] = new_model->data(new_model->index(newWaypointIdx-1, FlightDataModel::NED_EAST)).toDouble();
pos_prev[2] = new_model->data(new_model->index(newWaypointIdx-1, FlightDataModel::NED_DOWN)).toDouble();
// TODO: fix sign
float previous_radius = new_model->data(new_model->index(newWaypointIdx-1, FlightDataModel::MODE_PARAMS)).toDouble();
previous_curvature = (previous_radius < 1e-4) ? 0 : 1.0 / previous_radius;
} else {
// Use the home location as the starting point of paths.
pos_prev[0] = 0;
pos_prev[1] = 0;
pos_prev[2] = 0;
previous_curvature = 0;
}
// Get the settings for the upcoming waypoint
pos_next[0] = model->data(model->index(wpIdx+1, FlightDataModel::NED_NORTH)).toDouble();
pos_next[1] = model->data(model->index(wpIdx+1, FlightDataModel::NED_EAST)).toDouble();
pos_next[2] = model->data(model->index(wpIdx+1, FlightDataModel::NED_DOWN)).toDouble();
quint8 NextMode = model->data(model->index(wpIdx + 1, FlightDataModel::MODE), Qt::UserRole).toInt();
float NextModeParameter = model->data(model->index(wpIdx + 1, FlightDataModel::MODE_PARAMS), Qt::UserRole).toInt();
NextModeParameter = 0;
bool future_path_is_circle = NextMode == Waypoint::MODE_CIRCLEPOSITIONRIGHT ||
NextMode == Waypoint::MODE_CIRCLEPOSITIONLEFT;
// The vector in and out of the current waypoint
float q_future[3];
float q_future_mag = 0;
float q_current[3];
float q_current_mag = 0;
// In the case of line-line intersection lines, this is simply the direction of
// the old and new segments.
if (curvature == 0 &&
(NextModeParameter == 0 || future_path_is_circle)) { // Fixme: waypoint_future.ModeParameters needs to be replaced by waypoint_future.Mode. FOr this, we probably need a new function to handle the switch(waypoint.Mode)
// Vector from past to present switching locus
q_current[0] = pos_current[0] - pos_prev[0];
q_current[1] = pos_current[1] - pos_prev[1];
// Calculate vector from preset to future switching locus
q_future[0] = pos_next[0] - pos_current[0];
q_future[1] = pos_next[1] - pos_current[1];
}
//In the case of line-arc intersections, calculate the tangent of the new section.
else if (curvature == 0 &&
(NextModeParameter != 0 && !future_path_is_circle)) { // Fixme: waypoint_future.ModeParameters needs to be replaced by waypoint_future.Mode. FOr this, we probably need a new function to handle the switch(waypoint.Mode)
// Old segment: straight line
q_current[0] = pos_current[0] - pos_prev[0];
q_current[1] = pos_current[1] - pos_prev[1];
// New segment: Vector perpendicular to the vector from arc center to tangent point
bool clockwise = curvature > 0;
qint8 lambda;
if (clockwise == true) { // clockwise
lambda = 1;
} else { // counterclockwise
lambda = -1;
}
// Calculate circle center
float arcCenter_NE[2];
find_arc_center(pos_current, pos_next, 1.0f/curvature, arcCenter_NE, curvature > 0, true);
// Vector perpendicular to the vector from arc center to tangent point
q_future[0] = -lambda*(pos_current[1] - arcCenter_NE[1]);
q_future[1] = lambda*(pos_current[0] - arcCenter_NE[0]);
}
//In the case of arc-line intersections, calculate the tangent of the old section.
else if (curvature != 0 && (NextModeParameter == 0 || future_path_is_circle)) { // Fixme: waypoint_future.ModeParameters needs to be replaced by waypoint_future.Mode. FOr this, we probably need a new function to handle the switch(waypoint.Mode)
// Old segment: Vector perpendicular to the vector from arc center to tangent point
bool clockwise = previous_curvature > 0;
bool minor = true;
qint8 lambda;
if ((clockwise == true && minor == true) ||
(clockwise == false && minor == false)) { //clockwise minor OR counterclockwise major
lambda = 1;
} else { //counterclockwise minor OR clockwise major
lambda = -1;
}
// Calculate old circle center
float arcCenter_NE[2];
find_arc_center(pos_prev, pos_current,
1.0f/previous_curvature, arcCenter_NE, clockwise, minor);
// Vector perpendicular to the vector from arc center to tangent point
q_current[0] = -lambda*(pos_current[1] - arcCenter_NE[1]);
q_current[1] = lambda*(pos_current[0] - arcCenter_NE[0]);
// New segment: straight line
q_future [0] = pos_next[0] - pos_current[0];
q_future [1] = pos_next[1] - pos_current[1];
}
//In the case of arc-arc intersections, calculate the tangent of the old and new sections.
else if (curvature != 0 && (NextModeParameter != 0 && !future_path_is_circle)) { // Fixme: waypoint_future.ModeParameters needs to be replaced by waypoint_future.Mode. FOr this, we probably need a new function to handle the switch(waypoint.Mode)
// Old segment: Vector perpendicular to the vector from arc center to tangent point
bool clockwise = previous_curvature > 0;
bool minor = true;
qint8 lambda;
if ((clockwise == true && minor == true) ||
(clockwise == false && minor == false)) { //clockwise minor OR counterclockwise major
lambda = 1;
} else { //counterclockwise minor OR clockwise major
lambda = -1;
}
// Calculate old arc center
float arcCenter_NE[2];
find_arc_center(pos_prev, pos_current,
1.0f/previous_curvature, arcCenter_NE, clockwise, minor);
// New segment: Vector perpendicular to the vector from arc center to tangent point
q_current[0] = -lambda*(pos_prev[1] - arcCenter_NE[1]);
q_current[1] = lambda*(pos_prev[0] - arcCenter_NE[0]);
if (curvature > 0) { // clockwise
lambda = 1;
} else { // counterclockwise
lambda = -1;
}
// Calculate new arc center
find_arc_center(pos_current, pos_next, 1.0f/curvature, arcCenter_NE, curvature > 0, true);
// Vector perpendicular to the vector from arc center to tangent point
q_future[0] = -lambda*(pos_current[1] - arcCenter_NE[1]);
q_future[1] = lambda*(pos_current[0] - arcCenter_NE[0]);
}
q_current[2] = 0;
q_current_mag = VectorMagnitude(q_current); //Normalize
q_future[2] = 0;
q_future_mag = VectorMagnitude(q_future); //Normalize
// Normalize q_current and q_future
if (q_current_mag > 0) {
for (int i=0; i<3; i++)
q_current[i] = q_current[i]/q_current_mag;
}
if (q_future_mag > 0) {
for (int i=0; i<3; i++)
q_future[i] = q_future[i]/q_future_mag;
}
// Compute heading difference between current and future tangents.
float theta = angle_between_2d_vectors(q_current, q_future);
// Compute angle between current and future tangents.
float rho = circular_modulus_rad(theta - M_PI);
// Compute half angle
float rho2 = rho/2.0f;
// Circle the outside of acute angles
if (fabsf(rho) < M_PI/3.0f) {
float R = fillet_radius;
if (q_current_mag>0 && q_current_mag< R*sqrtf(3))
R = q_current_mag/sqrtf(3)-0.1f; // Remove 10cm to guarantee that no two points overlap.
if (q_future_mag >0 && q_future_mag < R*sqrtf(3))
R = q_future_mag /sqrtf(3)-0.1f; // Remove 10cm to guarantee that no two points overlap.
// The sqrt(3) term comes from the fact that the triangle that connects the center of
// the first/second arc with the center of the second/third arc is a 1-2-sqrt(3) triangle
float f1[3] = {pos_current[0] - R*q_current[0]*sqrtf(3), pos_current[1] - R*q_current[1]*sqrtf(3), pos_current[2]};
float f2[3] = {pos_current[0] + R*q_future[0]*sqrtf(3), pos_current[1] + R*q_future[1]*sqrtf(3), pos_current[2]};
// Add the waypoint segment
newWaypointIdx += addNonCircleToSwitchingLoci(f1, finalVelocity, curvature, newWaypointIdx);
float gamma = atan2f(q_current[1], q_current[0]);
// Compute eta, which is the angle between the horizontal and the center of the filleting arc f1 and
// sigma, which is the angle between the horizontal and the center of the filleting arc f2.
float eta;
float sigma;
if (theta > 0) { // Change in direction is clockwise, so fillets are clockwise
eta = gamma - M_PI/2.0f;
sigma = gamma + theta - M_PI/2.0f;
}
else {
eta = gamma + M_PI/2.0f;
sigma = gamma + theta + M_PI/2.0f;
}
// This starts the fillet into the circle
float pos[3] = {(pos_current[0] + f1[0] + R*cosf(eta))/2,
(pos_current[1] + f1[1] + R*sinf(eta))/2,
pos_current[2]};
setNewWaypoint(newWaypointIdx++, pos, finalVelocity, -SIGN(theta)*1.0f/R);
// This is the halfway point through the circle
pos[0] = pos_current[0] + R*cosf(gamma);
pos[1] = pos_current[1] + R*sinf(gamma);
pos[2] = pos_current[2];
setNewWaypoint(newWaypointIdx++, pos, finalVelocity, SIGN(theta)*1.0f/R);
// This is the transition from the circle to the fillet back onto the path
pos[0] = (pos_current[0] + (f2[0] + R*cosf(sigma)))/2;
pos[1] = (pos_current[1] + (f2[1] + R*sinf(sigma)))/2;
pos[2] = pos_current[2];
setNewWaypoint(newWaypointIdx++, pos, finalVelocity, SIGN(theta)*1.0f/R);
// This is the point back on the path
pos[0] = f2[0];
pos[1] = f2[1];
pos[2] = pos_current[2];
setNewWaypoint(newWaypointIdx++, pos, finalVelocity, -SIGN(theta)*1.0f/R);
}
else if (theta != 0) { // The two tangents have different directions
float R = fillet_radius;
// Remove 10cm to guarantee that no two points overlap. This would be better if we solved it by removing the next point instead.
if (q_current_mag>0 && q_current_mag<fabsf(R/tanf(rho2)))
R = qMin(R, q_current_mag*fabsf(tanf(rho2))-0.1f);
if (q_future_mag>0 && q_future_mag <fabsf(R/tanf(rho2)))
R = qMin(R, q_future_mag* fabsf(tanf(rho2))-0.1f);
// Add the waypoint segment
float f1[3];
f1[0] = pos_current[0] - R/fabsf(tanf(rho2))*q_current[0];
f1[1] = pos_current[1] - R/fabsf(tanf(rho2))*q_current[1];
f1[2] = pos_current[2];
newWaypointIdx += addNonCircleToSwitchingLoci(f1, finalVelocity, curvature, newWaypointIdx);
// Add the filleting segment in preparation for the next waypoint
float pos[3] = {pos_current[0] + R/fabsf(tanf(rho2))*q_future[0],
pos_current[1] + R/fabsf(tanf(rho2))*q_future[1],
pos_current[2]};
setNewWaypoint(newWaypointIdx++, pos, finalVelocity, SIGN(theta)*1.0f/R);
}
else {
// In this case, the two tangents are colinear
newWaypointIdx += addNonCircleToSwitchingLoci(pos_current, finalVelocity, curvature, newWaypointIdx);
}
}
else if (wpIdx == model->rowCount()-1)
// This is the final waypoint
newWaypointIdx += addNonCircleToSwitchingLoci(pos_current, finalVelocity, curvature, newWaypointIdx);
}
// Migrate the data to the original model now it is complete
model->replaceData(new_model);
delete new_model;
new_model = NULL;
return true;
}
void DSOUND_Calc3DBuffer(IDirectSoundBufferImpl *dsb)
{
/* volume, at which the sound will be played after all calcs. */
D3DVALUE lVolume = 0;
/* stuff for distance related stuff calc. */
D3DVECTOR vDistance;
D3DVALUE flDistance = 0;
/* panning related stuff */
D3DVALUE flAngle, flAngle2;
D3DVECTOR vLeft;
int i, num_main_speakers;
float a, ingain;
/* doppler shift related stuff */
TRACE("(%p)\n",dsb);
/* initial buffer volume */
lVolume = dsb->ds3db_lVolume;
switch (dsb->ds3db_ds3db.dwMode)
{
case DS3DMODE_DISABLE:
TRACE("3D processing disabled\n");
/* this one is here only to eliminate annoying warning message */
DSOUND_RecalcVolPan (&dsb->volpan);
return;
case DS3DMODE_NORMAL:
TRACE("Normal 3D processing mode\n");
/* we need to calculate distance between buffer and listener*/
vDistance = VectorBetweenTwoPoints(&dsb->device->ds3dl.vPosition, &dsb->ds3db_ds3db.vPosition);
flDistance = VectorMagnitude (&vDistance);
break;
case DS3DMODE_HEADRELATIVE:
TRACE("Head-relative 3D processing mode\n");
/* distance between buffer and listener is same as buffer's position */
vDistance = dsb->ds3db_ds3db.vPosition;
flDistance = VectorMagnitude (&vDistance);
break;
}
if (flDistance > dsb->ds3db_ds3db.flMaxDistance)
{
/* some apps don't want you to hear too distant sounds... */
if (dsb->dsbd.dwFlags & DSBCAPS_MUTE3DATMAXDISTANCE)
{
dsb->volpan.lVolume = DSBVOLUME_MIN;
DSOUND_RecalcVolPan (&dsb->volpan);
/* i guess mixing here would be a waste of power */
return;
}
else
flDistance = dsb->ds3db_ds3db.flMaxDistance;
}
if (flDistance < dsb->ds3db_ds3db.flMinDistance)
flDistance = dsb->ds3db_ds3db.flMinDistance;
/* attenuation proportional to the distance squared, converted to millibels as in lVolume*/
lVolume -= log10(flDistance/dsb->ds3db_ds3db.flMinDistance * flDistance/dsb->ds3db_ds3db.flMinDistance)*1000 * dsb->device->ds3dl.flRolloffFactor;
TRACE("dist. att: Distance = %f, MinDistance = %f => adjusting volume %d to %f\n", flDistance, dsb->ds3db_ds3db.flMinDistance, dsb->ds3db_lVolume, lVolume);
/* conning */
/* sometimes it happens that vConeOrientation vector = (0,0,0); in this case angle is "nan" and it's useless*/
if (dsb->ds3db_ds3db.vConeOrientation.x == 0 && dsb->ds3db_ds3db.vConeOrientation.y == 0 && dsb->ds3db_ds3db.vConeOrientation.z == 0)
{
TRACE("conning: cones not set\n");
}
else
{
D3DVECTOR vDistanceInv;
vDistanceInv.x = -vDistance.x;
vDistanceInv.y = -vDistance.y;
vDistanceInv.z = -vDistance.z;
/* calculate angle */
flAngle = AngleBetweenVectorsDeg(&dsb->ds3db_ds3db.vConeOrientation, &vDistanceInv);
/* if by any chance it happens that OutsideConeAngle = InsideConeAngle (that means that conning has no effect) */
if (dsb->ds3db_ds3db.dwInsideConeAngle != dsb->ds3db_ds3db.dwOutsideConeAngle)
{
/* my test show that for my way of calc., we need only half of angles */
DWORD dwInsideConeAngle = dsb->ds3db_ds3db.dwInsideConeAngle/2;
DWORD dwOutsideConeAngle = dsb->ds3db_ds3db.dwOutsideConeAngle/2;
if (dwOutsideConeAngle == dwInsideConeAngle)
++dwOutsideConeAngle;
/* full volume */
if (flAngle < dwInsideConeAngle)
flAngle = dwInsideConeAngle;
/* min (app defined) volume */
if (flAngle > dwOutsideConeAngle)
flAngle = dwOutsideConeAngle;
/* this probably isn't the right thing, but it's ok for the time being */
lVolume += ((flAngle - dwInsideConeAngle)/(dwOutsideConeAngle - dwInsideConeAngle)) * dsb->ds3db_ds3db.lConeOutsideVolume;
}
TRACE("conning: Angle = %f deg; InsideConeAngle(/2) = %d deg; OutsideConeAngle(/2) = %d deg; ConeOutsideVolume = %d => adjusting volume to %f\n",
flAngle, dsb->ds3db_ds3db.dwInsideConeAngle/2, dsb->ds3db_ds3db.dwOutsideConeAngle/2, dsb->ds3db_ds3db.lConeOutsideVolume, lVolume);
}
dsb->volpan.lVolume = lVolume;
ingain = pow(2.0, dsb->volpan.lVolume / 600.0) * 0xffff;
if (dsb->device->pwfx->nChannels == 1)
{
dsb->volpan.dwTotalAmpFactor[0] = ingain;
return;
}
/* panning */
if (vDistance.x == 0.0f && vDistance.y == 0.0f && vDistance.z == 0.0f)
flAngle = 0.0;
else
{
vLeft = VectorProduct(&dsb->device->ds3dl.vOrientFront, &dsb->device->ds3dl.vOrientTop);
/* To calculate angle to sound source we need to:
* 1) Get angle between vDistance and a plane on which angle to sound source should be 0.
* Such a plane is given by vectors vOrientFront and vOrientTop, and angle between vector
* and a plane equals to M_PI_2 - angle between vector and normal to this plane (vLeft in this case).
* 2) Determine if the source is behind or in front of us by calculating angle between vDistance
* and vOrientFront.
*/
flAngle = AngleBetweenVectorsRad(&vLeft, &vDistance);
flAngle2 = AngleBetweenVectorsRad(&dsb->device->ds3dl.vOrientFront, &vDistance);
if (flAngle2 > M_PI_2)
flAngle = -flAngle;
flAngle -= M_PI_2;
if (flAngle < -M_PI)
flAngle += 2*M_PI;
}
TRACE("panning: Angle = %f rad, lPan = %d\n", flAngle, dsb->volpan.lPan);
/* FIXME: Doppler Effect disabled since i have no idea which frequency to change and how to do it */
if(0)
{
D3DVALUE flFreq, flBufferVel, flListenerVel;
/* doppler shift*/
if (!VectorMagnitude(&dsb->ds3db_ds3db.vVelocity) && !VectorMagnitude(&dsb->device->ds3dl.vVelocity))
{
TRACE("doppler: Buffer and Listener don't have velocities\n");
}
else if (!(dsb->ds3db_ds3db.vVelocity.x == dsb->device->ds3dl.vVelocity.x &&
dsb->ds3db_ds3db.vVelocity.y == dsb->device->ds3dl.vVelocity.y &&
dsb->ds3db_ds3db.vVelocity.z == dsb->device->ds3dl.vVelocity.z))
{
/* calculate length of ds3db_ds3db.vVelocity component which causes Doppler Effect
NOTE: if buffer moves TOWARDS the listener, it's velocity component is NEGATIVE
if buffer moves AWAY from listener, it's velocity component is POSITIVE */
flBufferVel = ProjectVector(&dsb->ds3db_ds3db.vVelocity, &vDistance);
/* calculate length of ds3dl.vVelocity component which causes Doppler Effect
NOTE: if listener moves TOWARDS the buffer, it's velocity component is POSITIVE
if listener moves AWAY from buffer, it's velocity component is NEGATIVE */
flListenerVel = ProjectVector(&dsb->device->ds3dl.vVelocity, &vDistance);
/* formula taken from Gianicoli D.: Physics, 4th edition: */
/* FIXME: replace dsb->freq with appropriate frequency ! */
flFreq = dsb->freq * ((DEFAULT_VELOCITY + flListenerVel)/(DEFAULT_VELOCITY + flBufferVel));
TRACE("doppler: Buffer velocity (component) = %f, Listener velocity (component) = %f => Doppler shift: %d Hz -> %f Hz\n",
flBufferVel, flListenerVel, dsb->freq, flFreq);
/* FIXME: replace following line with correct frequency setting ! */
dsb->freq = flFreq;
DSOUND_RecalcFormat(dsb);
}
}
for (i = 0; i < dsb->device->pwfx->nChannels; i++)
dsb->volpan.dwTotalAmpFactor[i] = 0;
num_main_speakers = dsb->device->pwfx->nChannels;
if (dsb->device->lfe_channel != -1) {
dsb->volpan.dwTotalAmpFactor[dsb->device->lfe_channel] = ingain;
num_main_speakers--;
}
/* adapted from OpenAL's Alc/panning.c */
for (i = 0; i < num_main_speakers - 1; i++)
{
if(flAngle >= dsb->device->speaker_angles[i] && flAngle < dsb->device->speaker_angles[i+1])
{
/* Sound is between speakers i and i+1 */
a = (flAngle-dsb->device->speaker_angles[i]) / (dsb->device->speaker_angles[i+1]-dsb->device->speaker_angles[i]);
dsb->volpan.dwTotalAmpFactor[dsb->device->speaker_num[i]] = sqrtf(1.0f-a) * ingain;
dsb->volpan.dwTotalAmpFactor[dsb->device->speaker_num[i+1]] = sqrtf(a) * ingain;
return;
}
}
/* Sound is between last and first speakers */
if (flAngle < dsb->device->speaker_angles[0]) { flAngle += M_PI*2.0f; }
a = (flAngle-dsb->device->speaker_angles[i]) / (M_PI*2.0f + dsb->device->speaker_angles[0]-dsb->device->speaker_angles[i]);
dsb->volpan.dwTotalAmpFactor[dsb->device->speaker_num[i]] = sqrtf(1.0f-a) * ingain;
dsb->volpan.dwTotalAmpFactor[dsb->device->speaker_num[0]] = sqrtf(a) * ingain;
}
void DSOUND_Calc3DBuffer(IDirectSoundBufferImpl *dsb)
{
/* volume, at which the sound will be played after all calcs. */
D3DVALUE lVolume = 0;
/* stuff for distance related stuff calc. */
D3DVECTOR vDistance;
D3DVALUE flDistance = 0;
/* panning related stuff */
D3DVALUE flAngle;
D3DVECTOR vLeft;
/* doppler shift related stuff */
#if 0
D3DVALUE flFreq, flBufferVel, flListenerVel;
#endif
TRACE("(%p)\n",dsb);
/* initial buffer volume */
lVolume = dsb->ds3db_lVolume;
switch (dsb->ds3db_ds3db.dwMode)
{
case DS3DMODE_DISABLE:
TRACE("3D processing disabled\n");
/* this one is here only to eliminate annoying warning message */
DSOUND_RecalcVolPan (&dsb->volpan);
break;
case DS3DMODE_NORMAL:
TRACE("Normal 3D processing mode\n");
/* we need to calculate distance between buffer and listener*/
vDistance = VectorBetweenTwoPoints(&dsb->ds3db_ds3db.vPosition, &dsb->device->ds3dl.vPosition);
flDistance = VectorMagnitude (&vDistance);
break;
case DS3DMODE_HEADRELATIVE:
TRACE("Head-relative 3D processing mode\n");
/* distance between buffer and listener is same as buffer's position */
flDistance = VectorMagnitude (&dsb->ds3db_ds3db.vPosition);
break;
}
if (flDistance > dsb->ds3db_ds3db.flMaxDistance)
{
/* some apps don't want you to hear too distant sounds... */
if (dsb->dsbd.dwFlags & DSBCAPS_MUTE3DATMAXDISTANCE)
{
dsb->volpan.lVolume = DSBVOLUME_MIN;
DSOUND_RecalcVolPan (&dsb->volpan);
/* i guess mixing here would be a waste of power */
return;
}
else
flDistance = dsb->ds3db_ds3db.flMaxDistance;
}
if (flDistance < dsb->ds3db_ds3db.flMinDistance)
flDistance = dsb->ds3db_ds3db.flMinDistance;
/* attenuation proportional to the distance squared, converted to millibels as in lVolume*/
lVolume -= log10(flDistance/dsb->ds3db_ds3db.flMinDistance * flDistance/dsb->ds3db_ds3db.flMinDistance)*1000;
TRACE("dist. att: Distance = %f, MinDistance = %f => adjusting volume %d to %f\n", flDistance, dsb->ds3db_ds3db.flMinDistance, dsb->ds3db_lVolume, lVolume);
/* conning */
/* sometimes it happens that vConeOrientation vector = (0,0,0); in this case angle is "nan" and it's useless*/
if (dsb->ds3db_ds3db.vConeOrientation.x == 0 && dsb->ds3db_ds3db.vConeOrientation.y == 0 && dsb->ds3db_ds3db.vConeOrientation.z == 0)
{
TRACE("conning: cones not set\n");
}
else
{
/* calculate angle */
flAngle = AngleBetweenVectorsDeg(&dsb->ds3db_ds3db.vConeOrientation, &vDistance);
/* if by any chance it happens that OutsideConeAngle = InsideConeAngle (that means that conning has no effect) */
if (dsb->ds3db_ds3db.dwInsideConeAngle != dsb->ds3db_ds3db.dwOutsideConeAngle)
{
/* my test show that for my way of calc., we need only half of angles */
DWORD dwInsideConeAngle = dsb->ds3db_ds3db.dwInsideConeAngle/2;
DWORD dwOutsideConeAngle = dsb->ds3db_ds3db.dwOutsideConeAngle/2;
if (dwOutsideConeAngle == dwInsideConeAngle)
++dwOutsideConeAngle;
/* full volume */
if (flAngle < dwInsideConeAngle)
flAngle = dwInsideConeAngle;
/* min (app defined) volume */
if (flAngle > dwOutsideConeAngle)
flAngle = dwOutsideConeAngle;
/* this probably isn't the right thing, but it's ok for the time being */
lVolume += ((dsb->ds3db_ds3db.lConeOutsideVolume)/((dwOutsideConeAngle) - (dwInsideConeAngle))) * flAngle;
}
TRACE("conning: Angle = %f deg; InsideConeAngle(/2) = %d deg; OutsideConeAngle(/2) = %d deg; ConeOutsideVolume = %d => adjusting volume to %f\n",
flAngle, dsb->ds3db_ds3db.dwInsideConeAngle/2, dsb->ds3db_ds3db.dwOutsideConeAngle/2, dsb->ds3db_ds3db.lConeOutsideVolume, lVolume);
}
dsb->volpan.lVolume = lVolume;
/* panning */
if (dsb->device->ds3dl.vPosition.x == dsb->ds3db_ds3db.vPosition.x &&
dsb->device->ds3dl.vPosition.y == dsb->ds3db_ds3db.vPosition.y &&
dsb->device->ds3dl.vPosition.z == dsb->ds3db_ds3db.vPosition.z) {
dsb->volpan.lPan = 0;
flAngle = 0.0;
}
else
{
vDistance = VectorBetweenTwoPoints(&dsb->device->ds3dl.vPosition, &dsb->ds3db_ds3db.vPosition);
vLeft = VectorProduct(&dsb->device->ds3dl.vOrientFront, &dsb->device->ds3dl.vOrientTop);
flAngle = AngleBetweenVectorsRad(&vLeft, &vDistance);
/* for now, we'll use "linear formula" (which is probably incorrect); if someone has it in book, correct it */
dsb->volpan.lPan = 10000*2*flAngle/M_PI - 10000;
}
TRACE("panning: Angle = %f rad, lPan = %d\n", flAngle, dsb->volpan.lPan);
/* FIXME: Doppler Effect disabled since i have no idea which frequency to change and how to do it */
#if 0
/* doppler shift*/
if ((VectorMagnitude(&ds3db_ds3db.vVelocity) == 0) && (VectorMagnitude(&dsb->device->ds3dl.vVelocity) == 0))
{
TRACE("doppler: Buffer and Listener don't have velocities\n");
}
else if (ds3db_ds3db.vVelocity != dsb->device->ds3dl.vVelocity)
{
/* calculate length of ds3db_ds3db.vVelocity component which causes Doppler Effect
NOTE: if buffer moves TOWARDS the listener, it's velocity component is NEGATIVE
if buffer moves AWAY from listener, it's velocity component is POSITIVE */
flBufferVel = ProjectVector(&dsb->ds3db_ds3db.vVelocity, &vDistance);
/* calculate length of ds3dl.vVelocity component which causes Doppler Effect
NOTE: if listener moves TOWARDS the buffer, it's velocity component is POSITIVE
if listener moves AWAY from buffer, it's velocity component is NEGATIVE */
flListenerVel = ProjectVector(&dsb->device->ds3dl.vVelocity, &vDistance);
/* formula taken from Gianicoli D.: Physics, 4th edition: */
/* FIXME: replace dsb->freq with appropriate frequency ! */
flFreq = dsb->freq * ((DEFAULT_VELOCITY + flListenerVel)/(DEFAULT_VELOCITY + flBufferVel));
TRACE("doppler: Buffer velocity (component) = %lf, Listener velocity (component) = %lf => Doppler shift: %ld Hz -> %lf Hz\n", flBufferVel, flListenerVel,
dsb->freq, flFreq);
/* FIXME: replace following line with correct frequency setting ! */
dsb->freq = flFreq;
DSOUND_RecalcFormat(dsb);
DSOUND_MixToTemporary(dsb, 0, dsb->buflen);
}
#endif
/* time for remix */
DSOUND_RecalcVolPan(&dsb->volpan);
}
void VectorNormalize(vector_t *arg){
float mag = VectorMagnitude(arg);
arg->x = arg->x/mag;
arg->y = arg->y/mag;
arg->z = arg->z/mag;
}
/**
* Correct attitude drift. Choose from any of the following algorithms
*/
void updateAttitudeDrift(AccelsData * accelsData, GyrosData * gyrosData, const float delT, GlobalAttitudeVariables *glblAtt, AttitudeSettingsData *attitudeSettings, SensorSettingsData *inertialSensorSettings)
{
float *gyros = &gyrosData->x;
float *accels = &accelsData->x;
float omegaCorrP[3];
if (attitudeSettings->FilterChoice == ATTITUDESETTINGS_FILTERCHOICE_CCC) {
CottonComplementaryCorrection(accels, gyros, delT, glblAtt, omegaCorrP);
} else if (attitudeSettings->FilterChoice == ATTITUDESETTINGS_FILTERCHOICE_PREMERLANI ||
attitudeSettings->FilterChoice == ATTITUDESETTINGS_FILTERCHOICE_PREMERLANI_GPS) {
if (firstpass_flag) {
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
//Allocate memory for DCM drift globals
drft = (struct GlobalDcmDriftVariables *)
pvPortMalloc(sizeof (struct GlobalDcmDriftVariables));
memset(drft->GPSV_old, 0, sizeof(drft->GPSV_old));
memset(drft->omegaCorrI, 0, sizeof(drft->omegaCorrI));
memset(drft->accels_e_integrator, 0, sizeof(drft->accels_e_integrator));
// TODO: Expose these settings through UAVO
drft->accelsKp = 1;
drft->rollPitchKp = 20;
drft->rollPitchKi = 1;
drft->yawKp = 0;
drft->yawKi = 0;
drft->gyroCalibTau = 100;
// Set flags
if (module_state[MODULESETTINGS_ADMINSTATE_GPS] == MODULESETTINGS_ADMINSTATE_ENABLED && PIOS_COM_GPS) {
GPSVelocityConnectCallback(GPSVelocityUpdatedCb);
drft->gpsPresent_flag = true;
drft->gpsVelocityDataConsumption_flag = GPS_CONSUMED;
} else {
drft->gpsPresent_flag = false;
}
#if defined (PIOS_INCLUDE_MAGNETOMETER)
MagnetometerConnectCallback(MagnetometerUpdatedCb);
#endif
drft->magNewData_flag = false;
drft->delT_between_GPS = 0;
firstpass_flag = false;
}
// Apply arbitrary scaling to get into effective units
drft->rollPitchKp = glblAtt->accelKp * 1000.0f;
drft->rollPitchKi = glblAtt->accelKi * 10000.0f;
// Convert quaternions into rotation matrix
float Rbe[3][3];
Quaternion2R(glblAtt->q, Rbe);
#if defined (PIOS_INCLUDE_GPS)
if (attitudeSettings->FilterChoice == ATTITUDESETTINGS_FILTERCHOICE_PREMERLANI_GPS) {
Premerlani_GPS(accels, gyros, Rbe, delT, true, glblAtt, omegaCorrP);
} else if (attitudeSettings->FilterChoice == ATTITUDESETTINGS_FILTERCHOICE_PREMERLANI)
#endif
{
Premerlani_DCM(accels, gyros, Rbe, delT, false, glblAtt, omegaCorrP); //<-- GAWD, I HATE HOW FUNCTION ARGUMENTS JUST PILE UP. IT LOOKS UNPROFESSIONAL TO MIX INPUTS AND OUTPUTS
}
}
//Calibrate the gyroscopes.
//TODO: but only calibrate when system is armed.
if (0) { //<-- CURRENTLY DISABLE UNTIL TESTING CAN BE DONE.
float normOmegaScalar = VectorMagnitude(gyros);
calibrate_gyros_high_speed(gyros, omegaCorrP, normOmegaScalar, delT, inertialSensorSettings);
}
}
