double Calc::calcApparentSeparation()
{
double corrAz1, corrAz2, corrEl1, corrEl2;
F = alignAz[1];
H = alignEl[1];
ApparentToTrueAzEl();
corrAz1 = F;
corrEl1 = H;
F = alignAz[2];
H = alignEl[2];
ApparentToTrueAzEl();
corrAz2 = F;
corrEl2 = H;
return calcSeparation(corrAz1, corrEl1, corrAz2, corrEl2);
}
bool Calc::optimizeZ3()
{
double maxGuess;
double upperGuess, lowerGuess;
double upperError, lowerError;
// Our residual error threshold
const double maxErr = 0.01*radPerDeg;
int ntries;
// Calculate the true angular separation of the two stars,
// based on their RA/Dec. This need be calculated only once.
const double trueSep = calcSeparation(alignRA[1], alignDec[1], alignRA[2], alignDec[2]);
if (!MathLib::finite(trueSep)) {
printDebugMsg("Can't calculate star separation!");
return false;
}
if (trueSep < 5.0*radPerDeg) {
printDebugMsg("Stars too close together!");
return false;
}
maxGuess = 10.0*radPerDeg;
if (maxGuess > trueSep/2)
maxGuess = trueSep/2;
Z3 = upperGuess = maxGuess;
upperError = calcApparentSeparation() - trueSep;
if (MathLib::fabs(upperError) <= maxErr) {
printDebugMsg("Lucky pos. first guess");
return true; // Lucky guess!
}
Z3 = lowerGuess = -maxGuess;
lowerError = calcApparentSeparation() - trueSep;
if (MathLib::fabs(lowerError) <= maxErr) {
printDebugMsg("Lucky neg. first guess");
return true; // Lucky guess!
}
// For the interpolation to succeed, the upper and
// lower errors must be of opposite sign.
if (upperError * lowerError > 0.0) {
printDebugMsg("Correct Z3 out of range");
return false;
}
ntries = 0;
for (;;) {
double newGuess, newError;
ntries++;
// Interpolate between the upper and lower guesses to
// get a new guess.
Z3 = newGuess = lowerGuess +
((lowerError/(lowerError-upperError))*(upperGuess-lowerGuess));
newError = calcApparentSeparation() - trueSep;
if (MathLib::fabs(newError) <= maxErr) {
std::cout << "Converged after " << ntries << " tries: Z3= " << Z3*degPerRad << std::endl;
return true; // We have converged! Z3 is set.
}
if (ntries > 25) {
printDebugMsg("No convergence after 25 tries");
return false; // Failure to converge
}
// NewGuess replaces either lowerGuess or upperGuess, depending
// on the sign of the newError.
if (lowerError * newError > 0) {
// newError has the same sign as lowerError
lowerGuess = newGuess;
lowerError = newError;
} else {
upperGuess = newGuess;
upperError = newError;
}
}
// We should never get here!
printDebugMsg("Broke out of loop!?!");
return false;
}
// Same as above, but values in degrees.
double Calc::calcDegSeparation(double az1, double el1, double az2, double el2)
{
return calcSeparation(az1*radPerDeg, el1*radPerDeg,
az2*radPerDeg, el2*radPerDeg);
}
void NGLScene::updateMember(Member &io_toUpdate)
{
ngl::Vec3 alignment = calcAlignment(io_toUpdate) * 10;
ngl::Vec3 cohesion = /*followSphere->getPosition() - */calcCohesion(io_toUpdate) *10;
ngl::Vec3 separation = calcSeparation(io_toUpdate);
ngl::Vec3 newVelocity = (followSphere->getPosition()) -
(io_toUpdate.getPosition() +
(ngl::Vec3(alignment + cohesion + separation)));
for(unsigned int i = 0; i<ShapeStore.ShapeList.size(); i++)
{
float shapeDist = calcDistance(getMemberPosition(ShapeStore.ShapeList[i]), io_toUpdate.getPosition());
if(abs(shapeDist) < 8.0f)
{
newVelocity += (io_toUpdate.getPosition() - getMemberPosition(ShapeStore.ShapeList[i])) * 100;
}
}
if(newVelocity.lengthSquared() != 0.0f)
{
newVelocity.normalize();
}
//std::cout<<"x = "<<newVelocity.m_x<<" y = "<<newVelocity.m_y<<" z = "<<newVelocity.m_z<<"\n";
// if(newVelocity.m_x < 0.0f)
// {
// newVelocity.m_x = -newVelocity.m_x;
// }
// if(newVelocity.m_y < 0.0f)
// {
// newVelocity.m_y = -newVelocity.m_y;
// }
// if(newVelocity.m_z < 0.0f)
// {
// newVelocity.m_z = -newVelocity.m_z;
// }
float dist = ngl::Vec3(followSphere->getPosition() - io_toUpdate.getPosition()).length();
if (dist > 15.0f)
{
io_toUpdate.setForwardVector(followSphere->getPosition());
io_toUpdate.calcSideVector();
}
else
{
io_toUpdate.setSideVector(followSphere->getPosition());
io_toUpdate.calcForwardVector();
}
io_toUpdate.getForwardVector().normalize();
if(io_toUpdate.getForwardVector().m_x <0.0f)
{
io_toUpdate.setForwardVector(ngl::Vec3(-io_toUpdate.getForwardVector().m_x,
io_toUpdate.getForwardVector().m_y,
io_toUpdate.getForwardVector().m_z));
}
if(io_toUpdate.getForwardVector().m_y <0.0f)
{
io_toUpdate.setForwardVector(ngl::Vec3(io_toUpdate.getForwardVector().m_x,
-io_toUpdate.getForwardVector().m_y,
io_toUpdate.getForwardVector().m_z));
}
if(io_toUpdate.getForwardVector().m_z <0.0f)
{
io_toUpdate.setForwardVector(ngl::Vec3(io_toUpdate.getForwardVector().m_x,
io_toUpdate.getForwardVector().m_y,
-io_toUpdate.getForwardVector().m_z));
}
io_toUpdate.setVelocity(newVelocity * 0.0001f * (io_toUpdate.getForwardVector() + io_toUpdate.getPosition()) , false);
if(io_toUpdate.getVelocity().m_x > 0.5f)
{
io_toUpdate.setVelocity(ngl::Vec3(0.5f,
io_toUpdate.getVelocity().m_y,
io_toUpdate.getVelocity().m_z),
true);
}
else if(io_toUpdate.getVelocity().m_x < -0.5f)
{
io_toUpdate.setVelocity(ngl::Vec3(-0.5f,
io_toUpdate.getVelocity().m_y,
io_toUpdate.getVelocity().m_z),
true);
}
if(io_toUpdate.getVelocity().m_y > 0.5f)
{
io_toUpdate.setVelocity(ngl::Vec3(io_toUpdate.getVelocity().m_x,
0.5f,
io_toUpdate.getVelocity().m_z),
true);
}
else if(io_toUpdate.getVelocity().m_y < -0.5f)
{
io_toUpdate.setVelocity(ngl::Vec3(io_toUpdate.getVelocity().m_x,
-0.5f,
io_toUpdate.getVelocity().m_z),
true);
}
if(io_toUpdate.getVelocity().m_z > 0.5f)
{
io_toUpdate.setVelocity(ngl::Vec3(io_toUpdate.getVelocity().m_x,
io_toUpdate.getVelocity().m_y,
0.5f),
true);
}
else if(io_toUpdate.getVelocity().m_z < -0.5f)
{
io_toUpdate.setVelocity(ngl::Vec3(io_toUpdate.getVelocity().m_x,
io_toUpdate.getVelocity().m_y,
-0.5f),
true);
}
//io_toUpdate.setVelocity(newVelocity * 0.0001f * io_toUpdate.getForwardVector() , false);
io_toUpdate.setPosition(io_toUpdate.getVelocity(), false);
}
