/*wrapper calling rotation based on the current point */
double SplineTraveler::calcRotationAngle()
{
//rotationAngle = deriveRailAngle(0, .6, .3);
//compare rotation of traveler to an side axis vector
//calcRotation(Vector3f(1,0,0), path.splineLocation(curU, curPoint),
calcRotation(Vector3f(1,0,0), path.getNearbyPoint(-.2, curPoint, curU),
path.getNearbyPoint(.3, curPoint, curU));
return rotationAngle;
}
bool FlannMatcher::isKeyframe(Eigen::Matrix4f& H)
{
bool isKey;
double translation;
double angle[3];
calcTranslate(H,translation);
calcRotation(H,angle);
double angleThresh=5*M_PI/180;
double transThresh=0.1;
if(fabs(angle[0])>angleThresh || fabs(angle[1])>angleThresh ||
fabs(angle[2])>angleThresh || translation>transThresh)
isKey=true;
else
isKey=false;
return isKey;
}
/********************************************************************************
Update the camera for third person view
Vector3 newPosition is the new position which the camera is to be based on
********************************************************************************/
void TPCamera::UpdatePosition(Vector3 newPos, Vector3 newDir)
{
//Controls zoom with MMB
if (myKeys[VK_MBUTTON])
calcZoom();
//Rotate and adjust pitch with RMB
//if (myKeys[VK_RBUTTON])
{
//calcPitch();
calcRotation();
}
//else
{
//pitchChange = 0.f;
//angleChange = 0.f;
}
float theta = m_fTPVCameraAngle;
float offSet_X = calcHDist() * sin(Math::DegreeToRadian(theta));
float offSet_Z = calcHDist() * cos(Math::DegreeToRadian(theta));
//Target is at player
target.x = newPos.x + offSet_X * 2;
target.y = newPos.y + 5.f;
target.z = newPos.z + offSet_Z * 2;
//Focus mode
//if (myKeys[VK_RBUTTON])
//{
// position.x = newPos.x - offSet_X * 0.5f;
// position.y = newPos.y + calcVDist() * 0.5f;
// position.z = newPos.z - offSet_Z * 0.5f;
//}
//else
{
position.x = newPos.x - offSet_X;
position.y = newPos.y + calcVDist();
position.z = newPos.z - offSet_Z;
}
}
void QuatTrackBall::rotate(const Vec2f& new_v)
{
calcRotation(new_v);
do_spin();
}
void FGTrim::trimOnGround(void)
{
FGGroundReactions* GroundReactions = fdmex->GetGroundReactions();
FGPropagate* Propagate = fdmex->GetPropagate();
FGMassBalance* MassBalance = fdmex->GetMassBalance();
FGAccelerations* Accelerations = fdmex->GetAccelerations();
vector<ContactPoints> contacts;
FGLocation CGLocation = Propagate->GetLocation();
FGMatrix33 Tec2b = Propagate->GetTec2b();
FGMatrix33 Tl2b = Propagate->GetTl2b();
double hmin = 1E+10;
int contactRef = -1;
// Build the list of the aircraft contact points and take opportunity of the
// loop to find which one is closer to (or deeper into) the ground.
for (int i = 0; i < GroundReactions->GetNumGearUnits(); i++) {
ContactPoints c;
FGLGear* gear = GroundReactions->GetGearUnit(i);
c.location = gear->GetLocalGear();
FGLocation gearLoc = CGLocation.LocalToLocation(c.location);
c.location = Tl2b * c.location;
FGColumnVector3 normal, vDummy;
FGLocation lDummy;
double height = gearLoc.GetContactPoint(lDummy, normal, vDummy, vDummy);
c.normal = Tec2b * normal;
contacts.push_back(c);
if (height < hmin) {
hmin = height;
contactRef = i;
}
}
// Remove the contact point that is closest to the ground from the list:
// the rotation axis will be going thru this point so we need to remove it
// to avoid divisions by zero that could result from the computation of
// the rotations.
FGColumnVector3 contact0 = contacts[contactRef].location;
contacts.erase(contacts.begin() + contactRef);
// Update the initial conditions: this should remove the forces generated
// by overcompressed landing gears
fgic.SetAltitudeASLFtIC(fgic.GetAltitudeASLFtIC() - hmin);
fdmex->Initialize(&fgic);
fdmex->Run();
// Compute the rotation axis: it is obtained from the direction of the
// moment measured at the contact point 'contact0'
FGColumnVector3 force = MassBalance->GetMass() * Accelerations->GetUVWdot();
FGColumnVector3 moment = MassBalance->GetJ() * Accelerations->GetPQRdot()
+ force * contact0;
FGColumnVector3 rotationAxis = moment.Normalize();
// Compute the rotation parameters: angle and the first point to come into
// contact with the ground when the rotation is applied.
RotationParameters rParam = calcRotation(contacts, rotationAxis, contact0);
FGQuaternion q0(rParam.angleMin, rotationAxis);
// Apply the computed rotation to all the contact points
FGMatrix33 rot = q0.GetTInv();
vector<ContactPoints>::iterator iter;
for (iter = contacts.begin(); iter != contacts.end(); iter++)
iter->location = contact0 + rot * (iter->location - contact0);
// Remove the second point to come in contact with the ground from the list.
// The reason is the same than above: avoid divisions by zero when the next
// rotation will be computed.
FGColumnVector3 contact1 = rParam.contactRef->location;
contacts.erase(rParam.contactRef);
// Compute the rotation axis: now there are 2 points in contact with the
// ground so the only option for the aircraft is to rotate around the axis
// generated by these 2 points.
rotationAxis = contact1 - contact0;
// Make sure that the rotation orientation is consistent with the moment.
if (DotProduct(rotationAxis, moment) < 0.0)
rotationAxis = contact0 - contact1;
rotationAxis.Normalize();
// Compute the rotation parameters
rParam = calcRotation(contacts, rotationAxis, contact0);
FGQuaternion q1(rParam.angleMin, rotationAxis);
// Update the aircraft orientation
FGColumnVector3 euler = (q0 * q1 * fgic.GetOrientation()).GetEuler();
fgic.SetPhiRadIC(euler(1));
fgic.SetThetaRadIC(euler(2));
fgic.SetPsiRadIC(euler(3));
}
