TiXmlElement* TransformComponent::GenerateXml()
{
TiXmlElement* pBaseElement = CB_NEW TiXmlElement(GetName());
// initial transform -> position
TiXmlElement* pPosition = CB_NEW TiXmlElement("Position");
Vec3 pos(m_Transform.GetPosition());
pPosition->SetAttribute("x", ToStr(pos.x).c_str());
pPosition->SetAttribute("y", ToStr(pos.y).c_str());
pPosition->SetAttribute("z", ToStr(pos.z).c_str());
pBaseElement->LinkEndChild(pPosition);
// initial transform -> LookAt
TiXmlElement* pDirection = CB_NEW TiXmlElement("YawPitchRoll");
Vec3 orient(m_Transform.GetYawPitchRoll());
orient.x = RADIANS_TO_DEGREES(orient.x);
orient.y = RADIANS_TO_DEGREES(orient.y);
orient.z = RADIANS_TO_DEGREES(orient.z);
pDirection->SetAttribute("x", ToStr(orient.x).c_str());
pDirection->SetAttribute("y", ToStr(orient.y).c_str());
pDirection->SetAttribute("z", ToStr(orient.z).c_str());
pBaseElement->LinkEndChild(pDirection);
return pBaseElement;
}
tinyxml2::XMLElement* TransformComponent::VGenerateXml(tinyxml2::XMLDocument* pDoc)
{
tinyxml2::XMLElement* pBaseElement = pDoc->NewElement(VGetName());
// initial transform -> position
tinyxml2::XMLElement* pPosition = pDoc->NewElement("Position");
glm::vec3 pos = ::GetPosition(m_transform);
pPosition->SetAttribute("x", ToStr(pos.x).c_str());
pPosition->SetAttribute("y", ToStr(pos.y).c_str());
pPosition->SetAttribute("z", ToStr(pos.z).c_str());
pBaseElement->LinkEndChild(pPosition);
// initial transform -> LookAt
tinyxml2::XMLElement* pDirection = pDoc->NewElement("YawPitchRoll");
glm::vec3 eulerDegs = ::GetYawPitchRoll(m_transform);
eulerDegs.x = RADIANS_TO_DEGREES(eulerDegs.x);
eulerDegs.y = RADIANS_TO_DEGREES(eulerDegs.y);
eulerDegs.z = RADIANS_TO_DEGREES(eulerDegs.z);
pDirection->SetAttribute("x", ToStr(eulerDegs.x).c_str());
pDirection->SetAttribute("y", ToStr(eulerDegs.y).c_str());
pDirection->SetAttribute("z", ToStr(eulerDegs.z).c_str());
pBaseElement->LinkEndChild(pDirection);
return pBaseElement;
}
void Kamikaze::updateEnemy(Level * currLev) {
//todo move at ship;
if (!dead) {
Eigen::Vector3f shipLoc = currLev->ship->position;
Eigen::Vector3f direction = shipLoc - position;
direction.normalize();
direction /= 1000;
direction.z() -= currLev->ship->velocity.z();
Eigen::Vector3f mousePos = Eigen::Vector3f(shipLoc.x(), shipLoc.y(), shipLoc.z());
float rotateY = RADIANS_TO_DEGREES(atan((shipLoc.x() - position.x()) / (shipLoc.z() - position.z()))) - 90.0;
float rotateX = -RADIANS_TO_DEGREES(atan((shipLoc.y() - position.y()) / (shipLoc.z() - position.z())));
rotate.y() = rotateY;
rotate.x() = rotateX;
// if we're behind ship, keep going in that direction
if (direction.z() <= 0) {
direction[2] *= -1;
direction[2] += 0.2;
}
velocity = direction;
}
}
TiXmlElement* TransformComponent::VGenerateXml(void)
{
TiXmlElement* pBaseElement = AC_NEW TiXmlElement(VGetName());
// initial transform -> position
TiXmlElement* pPosition = AC_NEW TiXmlElement("Position");
Vec3 pos(m_transform.GetPosition());
pPosition->SetAttribute("x", ToStr(pos.x).c_str());
pPosition->SetAttribute("y", ToStr(pos.y).c_str());
pPosition->SetAttribute("z", ToStr(pos.z).c_str());
pBaseElement->LinkEndChild(pPosition);
// initial transform -> LookAt
TiXmlElement* pDirection = AC_NEW TiXmlElement("YawPitchRoll");
Vec3 orient(m_transform.GetYawPitchRoll());
orient.x = RADIANS_TO_DEGREES(orient.x);
orient.y = RADIANS_TO_DEGREES(orient.y);
orient.z = RADIANS_TO_DEGREES(orient.z);
pDirection->SetAttribute("x", ToStr(orient.x).c_str());
pDirection->SetAttribute("y", ToStr(orient.y).c_str());
pDirection->SetAttribute("z", ToStr(orient.z).c_str());
pBaseElement->LinkEndChild(pDirection);
/***
// This is not supported yet
// initial transform -> position
TiXmlElement* pScale = AC_NEW TiXmlElement("Scale");
pPosition->SetAttribute("x", ToStr(m_scale.x).c_str());
pPosition->SetAttribute("y", ToStr(m_scale.y).c_str());
pPosition->SetAttribute("z", ToStr(m_scale.z).c_str());
pBaseElement->LinkEndChild(pScale);
**/
return pBaseElement;
}
/**
* Update tracker orientation and position (if available).
* Updates tracker orientation and position (if available) and passes it to game mouse input accordingly.
***/
void MotionTracker::updateOrientationAndPosition()
{
#ifdef _DEBUG
OutputDebugString("Motion Tracker updateOrientationAndPosition\n");
#endif
// Get orientation from derived tracker.
if(getOrientationAndPosition(&yaw, &pitch, &roll, &x, &y, &z) == 0)
{
#ifdef _DEBUG
OutputDebugString("Motion Tracker getOrientation == 0\n");
#endif
// Skip empty input data.
if(!isEqual(currentYaw, 0.0f) && !isEqual(currentPitch, 0.0f))
{
// Convert yaw, pitch to positive degrees, multiply by multiplier.
// (-180.0f...0.0f -> 180.0f....360.0f)
// (0.0f...180.0f -> 0.0f...180.0f)
yaw = fmodf(RADIANS_TO_DEGREES(yaw) + 360.0f, 360.0f)*multiplierYaw;
pitch = -fmodf(RADIANS_TO_DEGREES(pitch) + 360.0f, 360.0f)*multiplierPitch;
// Get difference.
deltaYaw += yaw - currentYaw;
deltaPitch += pitch - currentPitch;
// Set limits.
if(fabs(deltaYaw) > 100.0f) deltaYaw = 0.0f;
if(fabs(deltaPitch) > 100.0f) deltaPitch = 0.0f;
// Pass to mouse data (long integer).
mouseData.mi.dx = (long)(deltaYaw);
mouseData.mi.dy = (long)(deltaPitch);
// Keep fractional difference in the delta so it's added to the next update.
deltaYaw -= (float)mouseData.mi.dx;
deltaPitch -= (float)mouseData.mi.dy;
#ifdef _DEBUG
//OutputDebugString("Motion Tracker SendInput\n");
#endif
// Send to mouse input.
if (mouseEmulation)
SendInput(1, &mouseData, sizeof(INPUT));
}
// Set current data.
currentYaw = yaw;
currentPitch = pitch;
currentRoll = roll*multiplierRoll;
}
}
float calcDihed(vector<float> &a, vector<float> &b, vector<float> &c, vector<float> &d) {
//cout << "a " << a[0] <<" "<< a[1] <<" "<< a[2] << endl;
//cout << "b " << b[0] <<" "<< b[1] <<" "<< b[2] << endl;
//cout << "c " << c[0] <<" "<< c[1] <<" "<< c[2] << endl;
//cout << "d " << d[0] <<" "<< d[1] <<" "<< d[2] << endl;
vector<float> ba(3), bc(3), cb(3), cd(3);
point_sub(ba, a, b); point_sub(bc, c, b);
point_sub(cb, b, c); point_sub(cd, d, c);
//cout << "ba " << ba[0] <<" "<< ba[1] <<" "<< ba[2] << endl;
//cout << "bc " << bc[0] <<" "<< bc[1] <<" "<< bc[2] << endl;
//cout << "cb " << cb[0] <<" "<< cb[1] <<" "<< cb[2] << endl;
//cout << "cd " << cd[0] <<" "<< cd[1] <<" "<< cd[2] << endl;
vector<float> ba_bc(3), cb_cd(3);
cross_product(cb_cd, cb, cd); normalize_point(cb_cd, cb_cd);
cross_product(ba_bc, ba, bc); normalize_point(ba_bc, ba_bc);
//cout << "cb_cd " << cb_cd[0] <<" "<< cb_cd[1] <<" "<< cb_cd[2] << endl;
//cout << "ba_bc " << ba_bc[0] <<" "<< ba_bc[1] <<" "<< ba_bc[2] << endl;
float dp = dot_product(cb_cd, ba_bc);
if(dp > 1) dp = 1;
if(dp < -1) dp = -1;
float angle = RADIANS_TO_DEGREES ( acos(dp) );
//cout << angle <<" "<< dp << endl;
vector<float> cp(3); cross_product(cp, ba_bc,cb_cd);
if ( dot_product(cp,bc) < 0 ) angle = -1*angle;
return angle;
}
//------------------------------------------------------------------------------
void CircleTreeDrawer::DrawInternal (Node *p)
{
NodePtr anc = p->GetAnc();
if (anc)
{
DrawLine (node_coordinates[p], node_backarc[p]);
double left_angle = node_angle[p->GetChild()];
double right_angle = node_angle[p->GetChild()->GetRightMostSibling()];
#if USE_PS
GPoint pt;
pt.SetX((int)origin.x);
pt.SetY((int)origin.y);
Port.DrawArc (pt, node_radius[p], int(RADIANS_TO_DEGREES(left_angle)), RADIANS_TO_DEGREES(right_angle)); //added int() around Radians to Degrees to comply with GCC 4.0 (added by BCO)
#endif
}
DrawInternalLabel (p);
}
bool TransformComponent::Init(TiXmlElement* pData)
{
CB_ASSERT(pData);
Vec3 yawPitchRoll = m_Transform.GetYawPitchRoll();
yawPitchRoll.x = RADIANS_TO_DEGREES(yawPitchRoll.x);
yawPitchRoll.y = RADIANS_TO_DEGREES(yawPitchRoll.y);
yawPitchRoll.z = RADIANS_TO_DEGREES(yawPitchRoll.z);
Vec3 position = m_Transform.GetPosition();
TiXmlElement* pPositionElement = pData->FirstChildElement("Position");
if (pPositionElement)
{
double x = 0;
double y = 0;
double z = 0;
pPositionElement->Attribute("x", &x);
pPositionElement->Attribute("y", &y);
pPositionElement->Attribute("z", &z);
position = Vec3(x, y, z);
}
TiXmlElement* pOrientationElement = pData->FirstChildElement("YawPitchRoll");
if (pOrientationElement)
{
double yaw = 0;
double pitch = 0;
double roll = 0;
pOrientationElement->Attribute("x", &yaw);
pOrientationElement->Attribute("y", &pitch);
pOrientationElement->Attribute("z", &roll);
yawPitchRoll = Vec3(yaw, pitch, roll);
}
Mat4x4 translation;
translation.BuildTranslation(position);
Mat4x4 rotation;
rotation.BuildYawPitchRoll((float)DEGREES_TO_RADIANS(yawPitchRoll.x), (float)DEGREES_TO_RADIANS(yawPitchRoll.y), (float)DEGREES_TO_RADIANS(yawPitchRoll.z));
m_Transform = rotation * translation;
return true;
}
float calcAngle(vector<float> & A, vector<float> & B, vector<float> & C) {
vector<float> BA(3), BC(3);
linear_combination(BA, 1, A, -1, B);
linear_combination(BC, 1, C, -1, B);
normalize_point(BA, BA); normalize_point(BC, BC);
float dp = dot_product(BA,BC);
if(dp > 1) dp = 1;
else if(dp < -1) dp = -1;
return RADIANS_TO_DEGREES( acos(dp) );
}
/* Get forecast into newly alloc'ed string */
gboolean
iwin_start_open (GWeatherInfo *info)
{
GWeatherInfoPrivate *priv;
gchar *url;
WeatherLocation *loc;
SoupMessage *msg;
g_assert (info != NULL);
priv = info->priv;
loc = &priv->location;
/* No zone (or -) means no weather information from national offices.
We don't actually use zone, but it's a good indicator of a US location.
(@ and : prefixes were used in the past for Australia and UK) */
if (!loc->zone || loc->zone[0] == '-' || loc->zone[0] == '@' || loc->zone[0] == ':')
return FALSE;
if (!loc->latlon_valid)
return FALSE;
/* see the description here: http://www.weather.gov/forecasts/xml/ */
struct tm tm;
time_t now;
gchar latstr[G_ASCII_DTOSTR_BUF_SIZE], lonstr[G_ASCII_DTOSTR_BUF_SIZE];
now = time (NULL);
localtime_r (&now, &tm);
g_ascii_dtostr (latstr, sizeof(latstr), RADIANS_TO_DEGREES (loc->latitude));
g_ascii_dtostr (lonstr, sizeof(lonstr), RADIANS_TO_DEGREES (loc->longitude));
url = g_strdup_printf ("http://www.weather.gov/forecasts/xml/sample_products/browser_interface/ndfdBrowserClientByDay.php?&lat=%s&lon=%s&format=24+hourly&startDate=%04d-%02d-%02d&numDays=7",
latstr, lonstr, 1900 + tm.tm_year, 1 + tm.tm_mon, tm.tm_mday);
msg = soup_message_new ("GET", url);
_gweather_info_begin_request (info, msg);
soup_session_queue_message (priv->session, msg, iwin_finish, info);
g_free (url);
return TRUE;
}
void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, uint32_t currentTime)
{
int16_t pitchCorrection = 0; // >0 climb, <0 dive
int16_t rollCorrection = 0; // >0 right, <0 left
int16_t throttleCorrection = 0; // raw throttle
int16_t minThrottleCorrection = posControl.navConfig->fw_min_throttle - posControl.navConfig->fw_cruise_throttle;
int16_t maxThrottleCorrection = posControl.navConfig->fw_max_throttle - posControl.navConfig->fw_cruise_throttle;
// Mix Pitch/Roll/Throttle
if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
pitchCorrection += posControl.rcAdjustment[PITCH];
throttleCorrection += DECIDEGREES_TO_DEGREES(posControl.rcAdjustment[PITCH]) * posControl.navConfig->fw_pitch_to_throttle;
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
}
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
pitchCorrection += ABS(posControl.rcAdjustment[ROLL]) * (posControl.navConfig->fw_roll_to_pitch / 100.0f);
rollCorrection += posControl.rcAdjustment[ROLL];
}
// Speed controller - only apply in POS mode
if (navStateFlags & NAV_CTL_POS) {
throttleCorrection += applyFixedWingMinSpeedController(currentTime);
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
}
// Limit and apply
if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
// PITCH angle is measured in opposite direction ( >0 - dive, <0 - climb)
pitchCorrection = constrain(pitchCorrection, -DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_dive_angle), DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_climb_angle));
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, posControl.pidProfile->max_angle_inclination[FD_PITCH]);
}
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
rollCorrection = constrain(rollCorrection, -DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_bank_angle), DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_bank_angle));
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, posControl.pidProfile->max_angle_inclination[FD_ROLL]);
// Calculate coordinated turn rate based on velocity and banking angle
if (posControl.actualState.velXY >= 300.0f) {
float targetYawRateDPS = RADIANS_TO_DEGREES(tan_approx(DECIDEGREES_TO_RADIANS(-rollCorrection)) * GRAVITY_CMSS / posControl.actualState.velXY);
rcCommand[YAW] = pidRateToRcCommand(targetYawRateDPS, currentControlRateProfile->rates[FD_YAW]);
}
else {
rcCommand[YAW] = 0;
}
}
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
uint16_t correctedThrottleValue = constrain(posControl.navConfig->fw_cruise_throttle + throttleCorrection, posControl.navConfig->fw_min_throttle, posControl.navConfig->fw_max_throttle);
rcCommand[THROTTLE] = constrain(correctedThrottleValue, posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle);
}
}
gboolean
owm_start_open (GWeatherInfo *info)
{
GWeatherInfoPrivate *priv;
gchar *url;
SoupMessage *message;
WeatherLocation *loc;
gchar latstr[G_ASCII_DTOSTR_BUF_SIZE], lonstr[G_ASCII_DTOSTR_BUF_SIZE];
priv = info->priv;
loc = &priv->location;
if (!loc->latlon_valid)
return FALSE;
/* see the description here: http://bugs.openweathermap.org/projects/api/wiki/Api_2_5_forecast */
g_ascii_dtostr (latstr, sizeof(latstr), RADIANS_TO_DEGREES (loc->latitude));
g_ascii_dtostr (lonstr, sizeof(lonstr), RADIANS_TO_DEGREES (loc->longitude));
#define TEMPLATE_START "http://api.openweathermap.org/data/2.5/forecast?lat=%s&lon=%s&mode=xml&units=metric"
#ifdef OWM_APIKEY
#define TEMPLATE TEMPLATE_START "&APPID=" OWM_APIKEY
#else
#define TEMPLATE TEMPLATE_START
#endif
url = g_strdup_printf (TEMPLATE, latstr, lonstr);
#undef TEMPLATE
message = soup_message_new ("GET", url);
soup_session_queue_message (priv->session, message, owm_finish, info);
priv->requests_pending++;
g_free (url);
return TRUE;
}
MovementController::MovementController(shared_ptr<SceneNode> object, float initialYaw, float initialPitch, bool rotateWhenLButtonDown)
: m_object(object)
{
m_object->VGet()->Transform(&m_matToWorld, &m_matFromWorld);
m_fTargetYaw = m_fYaw = RADIANS_TO_DEGREES(-initialYaw);
m_fTargetPitch = m_fPitch = RADIANS_TO_DEGREES(initialPitch);
m_maxSpeed = 30.0f; // 30 meters per second
m_currentSpeed = 0.0f;
Vec3 pos = m_matToWorld.GetPosition();
m_matPosition.BuildTranslation(pos);
POINT ptCursor;
GetCursorPos( &ptCursor );
m_lastMousePos = ptCursor;
memset(m_bKey, 0x00, sizeof(m_bKey));
m_mouseLButtonDown = false;
m_bRotateWhenLButtonDown = rotateWhenLButtonDown;
}
int FreeSpaceTracker::getOrientationFromDevice(float* yaw, float* pitch, float* roll)
{
//OutputDebugString("Free Tracker getOrient\n");
freespace_message msg;
int err = freespace_readMessage(DeviceID, &msg, 10);
/*
char errChar[512];
sprintf_s(errChar, "devID = %d, err == %d", DeviceID, err);
OutputDebugString(errChar);
OutputDebugString("\n");
*/
if (err == 0)
{
// Check if this is a user frame message.
if (msg.messageType == FREESPACE_MESSAGE_USERFRAME)
{
// Convert from quaternion to Euler angles
// Get the quaternion vector
float w = msg.userFrame.angularPosA;
float x = msg.userFrame.angularPosB;
float y = msg.userFrame.angularPosC;
float z = msg.userFrame.angularPosD;
// normalize the vector
float len = sqrtf((w*w) + (x*x) + (y*y) + (z*z));
w /= len;
x /= len;
y /= len;
z /= len;
// The Freespace quaternion gives the rotation in terms of
// rotating the world around the object. We take the conjugate to
// get the rotation in the object's reference frame.
w = w;
x = -x;
y = -y;
z = -z;
// Convert to angles in radians
float m11 = (2.0f * w * w) + (2.0f * x * x) - 1.0f;
float m12 = (2.0f * x * y) + (2.0f * w * z);
float m13 = (2.0f * x * z) - (2.0f * w * y);
float m23 = (2.0f * y * z) + (2.0f * w * x);
float m33 = (2.0f * w * w) + (2.0f * z * z) - 1.0f;
*roll = RADIANS_TO_DEGREES(atan2f(m23, m33));
*pitch = RADIANS_TO_DEGREES(asinf(-m13));
*yaw = RADIANS_TO_DEGREES(atan2f(m12, m11));
return 0;
}
// any other message types will just fall through and keep looping until the timeout is reached
}
else
return err; // return on timeouts or serious errors
return FREESPACE_ERROR_TIMEOUT; // The function returns gracefully without values
}
void Scanner::run()
{
// Prepare to scan
prepareScan();
// Set the base output file
std::stringstream sstr;
sstr << SCAN_OUTPUT_DIR << std::string("/") << time(NULL);
m_filename = sstr.str();
m_firstRowRightLaserCol = m_camera->getImageWidth() * 0.5;
m_firstRowLeftLaserCol = m_camera->getImageWidth() * 0.5;
Scanner::TimingStats timingStats;
memset(&timingStats, 0, sizeof(Scanner::TimingStats));
timingStats.startTime = GetTimeInSeconds();
double time1 = 0;
Setup * setup = Setup::get();
Preset preset = PresetManager::get()->getActivePreset();
// Read the laser selection
m_laserSelection = preset.laserSide;
// Read the location of the lasers and camera
m_rightLaserLoc = setup->rightLaserLocation;
m_leftLaserLoc = setup->leftLaserLocation;
m_cameraLoc = setup->cameraLocation;
LocationMapper leftLocMapper(m_leftLaserLoc, m_cameraLoc);
LocationMapper rightLocMapper(m_rightLaserLoc, m_cameraLoc);
// Compute the angle between the two laser planes
real leftLaserX = ABS(m_leftLaserLoc.x);
real rightLaserX = ABS(m_rightLaserLoc.x);
real camZ = ABS(m_cameraLoc.z);
// Sanity check to prevent divide by 0. In reality the laser should never be this close to the camera
if (leftLaserX < 0.001)
{
leftLaserX = 0.001;
}
if (rightLaserX < 0.001)
{
rightLaserX = 0.001;
}
//
// tan(theta) = ABS(laserX) / camZ
// theta = atan(ABS(laserX) / camZ)
//
real leftLaserAngle = atan(leftLaserX / camZ);
real rightLaserAngle = atan(rightLaserX / camZ);
m_radiansBetweenLaserPlanes = leftLaserAngle + rightLaserAngle;
// Write the range CSV
if (m_writeRangeCsvEnabled)
{
m_rangeFout.open((m_filename + ".csv").c_str());
if (!m_rangeFout.is_open())
{
throw Exception("Error opening range CSV file");
}
}
// Init the results vectors
m_results.enter();
m_leftLaserResults.clear();
m_rightLaserResults.clear();
m_results.leave();
int numFrames = 0;
int maxFramesPerRevolution = preset.framesPerRevolution;
if (maxFramesPerRevolution < 1)
{
maxFramesPerRevolution = 1;
}
int stepsPerRevolution = Setup::get()->stepsPerRevolution;
if (maxFramesPerRevolution > stepsPerRevolution)
{
maxFramesPerRevolution = stepsPerRevolution;
}
try
{
// Enable the turn table motor
m_turnTable->setMotorEnabled(true);
std::cout << "Enabled motor" << std::endl;
if (m_laser == NULL)
{
throw Exception("Laser object is NULL");
}
if (m_turnTable == NULL)
{
throw Exception("Laser object is NULL");
}
float rotation = 0;
// Read the number of motor steps per revolution
int stepsPerRevolution = setup->stepsPerRevolution;
float rangeRadians = (m_range / 360) * (2 * PI);
// The number of steps for a single frame
int stepsPerFrame = ceil(stepsPerRevolution / (float)maxFramesPerRevolution);
if (stepsPerFrame < 1)
{
stepsPerFrame = 1;
}
// The number of radians a single step takes you
m_radiansPerFrame = ((2 * PI) / (float) stepsPerRevolution);
// The radians to move for a single frame
float frameRadians = stepsPerFrame * m_radiansPerFrame;
numFrames = ceil(rangeRadians / frameRadians);
m_numFramesBetweenLaserPlanes = m_radiansBetweenLaserPlanes / frameRadians;
std::cout << "Angle between laser planes: " << RADIANS_TO_DEGREES(m_radiansBetweenLaserPlanes)
<< " degrees, radiansPerFrame=" << m_radiansPerFrame
<< ", numFramesBetweenLaserPlanes=" << m_numFramesBetweenLaserPlanes
<< ", numFrames=" << numFrames << std::endl;
for (int iFrame = 0; iFrame < numFrames; iFrame++)
{
timingStats.numFrames++;
// Stop if the user asked us to
if (m_stopRequested)
{
break;
}
singleScan(iFrame, rotation, frameRadians, leftLocMapper, rightLocMapper, &timingStats);
rotation += frameRadians;
// Update the progress
double progress = (iFrame + 1.0) / numFrames;
double timeElapsed = GetTimeInSeconds() - m_startTimeSec;
double percentComplete = 100.0 * progress;
double percentPerSecond = percentComplete / timeElapsed;
double fullTimeSec = 100.0 / percentPerSecond;
double remainingSec = fullTimeSec - timeElapsed;
m_progress.setPercent(progress * 100);
m_status.enter();
m_remainingTime = remainingSec;
m_status.leave();
logTimingStats(timingStats);
std::cout << percentComplete << "% Complete, " << (remainingSec / 60) << " minutes remaining." << std::endl;
}
}
catch (...)
{
m_turnTable->setMotorEnabled(false);
m_status.enter();
m_running = false;
m_status.leave();
if (m_writeRangeCsvEnabled)
{
m_rangeFout.close();
}
throw;
}
m_rangeFout.close();
m_turnTable->setMotorEnabled(false);
std::cout << "Merging laser results..." << std::endl;
time1 = GetTimeInSeconds();
// Merge the left and right lasers and sort the results
std::vector<NeutralFileRecord> results;
LaserResultsMerger merger;
m_results.enter();
merger.merge(results, m_leftLaserResults, m_rightLaserResults, maxFramesPerRevolution,
m_numFramesBetweenLaserPlanes, Camera::getInstance()->getImageHeight(), preset.laserMergeAction, m_progress);
m_results.leave();
// Sort by pseudo-step and row
std::cout << "Sort 2... " << std::endl;
std::sort(results.begin(), results.end(), ComparePseudoSteps);
std::cout << "End Sort 2... " << std::endl;
m_results.enter();
std::cout << "Merged " << m_leftLaserResults.size() << " left laser and " << m_rightLaserResults.size() << " right laser results into " << results.size() << " results." << std::endl;
m_leftLaserResults.clear();
m_rightLaserResults.clear();
m_results.leave();
std::cout << "Constructing mesh..." << std::endl;
timingStats.laserMergeTime += GetTimeInSeconds() - time1;
// Write the PLY file
std::cout << "Starting output thread..." << std::endl;
if (preset.generatePly)
{
m_progress.setLabel("Generating PLY file");
m_progress.setPercent(0);
std::string plyFilename = m_filename + ".ply";
std::cout << "Writing PLY file... " << plyFilename << std::endl;
time1 = GetTimeInSeconds();
FileWriter plyOut(plyFilename.c_str());
if (!plyOut.is_open())
{
throw Exception("Error opening file for writing: " + plyFilename);
}
/** Writes the results to a PLY file */
PlyWriter plyWriter;
plyWriter.setDataFormat(preset.plyDataFormat);
plyWriter.setTotalNumPoints((int)results.size());
plyWriter.begin(&plyOut);
real percent = 0;
for (size_t iRec = 0; iRec < results.size(); iRec++)
{
real newPct = 100.0f * iRec / results.size();
if (newPct - percent > 0.1)
{
m_progress.setPercent(newPct);
percent = newPct;
}
plyWriter.writePoints(&results[iRec].point, 1);
}
plyWriter.end();
plyOut.close();
timingStats.pointCloudWritingTime += GetTimeInSeconds() - time1;
}
// Generate the XYZ file
if (preset.generateXyz)
{
std::cout << "Generating XYZ file..." << std::endl;
time1 = GetTimeInSeconds();
XyzWriter xyzWriter;
xyzWriter.write(m_filename, results, m_progress);
timingStats.pointCloudWritingTime += GetTimeInSeconds() - time1;
}
// Generate the STL file
if (preset.generateStl)
{
std::cout << "Generating STL mesh..." << std::endl;
time1 = GetTimeInSeconds();
StlWriter stlWriter;
stlWriter.write(m_filename, results, m_range > 359, m_progress);
timingStats.meshWritingTime = GetTimeInSeconds() - time1;
}
logTimingStats(timingStats);
m_progress.setPercent(100);
m_status.enter();
m_running = false;
m_status.leave();
std::cout << "Done." << std::endl;
}
float calcAngle(vector<float> & A, vector<float> & B) {
vector<float> a = A, b = B;
normalize_point(a, A); normalize_point(b, B);
return RADIANS_TO_DEGREES(acos(dot_product(A,B)));
}
float_t SimpleCamera::getFieldOfViewInDegree() const
{
return RADIANS_TO_DEGREES(_fovInRad);
}
bool TransformComponent::VInit(TiXmlElement* pData)
{
AC_ASSERT(pData);
// [mrmike] - this was changed post-press - because changes to the TransformComponents can come in partial definitions,
// such as from the editor, its better to grab the current values rather than clear them out.
Vec3 yawPitchRoll = m_transform.GetYawPitchRoll();
yawPitchRoll.x = RADIANS_TO_DEGREES(yawPitchRoll.x);
yawPitchRoll.y = RADIANS_TO_DEGREES(yawPitchRoll.y);
yawPitchRoll.z = RADIANS_TO_DEGREES(yawPitchRoll.z);
Vec3 position = m_transform.GetPosition();
TiXmlElement* pPositionElement = pData->FirstChildElement("Position");
if (pPositionElement)
{
double x = 0;
double y = 0;
double z = 0;
pPositionElement->Attribute("x", &x);
pPositionElement->Attribute("y", &y);
pPositionElement->Attribute("z", &z);
position = Vec3(x, y, z);
}
TiXmlElement* pOrientationElement = pData->FirstChildElement("YawPitchRoll");
if (pOrientationElement)
{
double yaw = 0;
double pitch = 0;
double roll = 0;
pOrientationElement->Attribute("x", &yaw);
pOrientationElement->Attribute("y", &pitch);
pOrientationElement->Attribute("z", &roll);
yawPitchRoll = Vec3(yaw, pitch, roll);
}
Mat4x4 translation;
translation.BuildTranslation(position);
Mat4x4 rotation;
rotation.BuildYawPitchRoll((float)DEGREES_TO_RADIANS(yawPitchRoll.x), (float)DEGREES_TO_RADIANS(yawPitchRoll.y), (float)DEGREES_TO_RADIANS(yawPitchRoll.z));
/**
// This is not supported yet.
TiXmlElement* pLookAtElement = pData->FirstChildElement("LookAt");
if (pLookAtElement)
{
double x = 0;
double y = 0;
double z = 0;
pLookAtElement->Attribute("x", &x);
pLookAtElement->Attribute("y", &y);
pLookAtElement->Attribute("z", &z);
Vec3 lookAt((float)x, (float)y, (float)z);
rotation.BuildRotationLookAt(translation.GetPosition(), lookAt, g_Up);
}
TiXmlElement* pScaleElement = pData->FirstChildElement("Scale");
if (pScaleElement)
{
double x = 0;
double y = 0;
double z = 0;
pScaleElement->Attribute("x", &x);
pScaleElement->Attribute("y", &y);
pScaleElement->Attribute("z", &z);
m_scale = Vec3((float)x, (float)y, (float)z);
}
**/
m_transform = rotation * translation;
return true;
}
/*
* calc_moon_internal:
* @info: WeatherInfo containing time_t of interest. The
* values moonphase, moonlatitude and moonValid are updated
* on success.
*
* Returns: gboolean indicating success or failure.
* moonphase is expressed as degrees where '0' is a new moon,
* '90' is first quarter, etc.
*/
static gboolean
calc_moon_internal (time_t update, gdouble *moonphase, gdouble *moonlatitude)
{
time_t t;
gdouble ra_h;
gdouble decl_r;
gdouble ndays, sunMeanAnom_d;
gdouble moonLong_d;
gdouble moonMeanAnom_d, moonMeanAnom_r;
gdouble sunEclipLong_r;
gdouble ascNodeMeanLong_d;
gdouble corrLong_d, eviction_d;
gdouble sinSunMeanAnom;
gdouble Ae, A3, Ec, A4, lN_r;
gdouble lambda_r, beta_r;
/*
* The comments refer to the enumerated steps to calculate the
* position of the moon (section 65 of above reference)
*/
t = update;
ndays = EPOCH_TO_J2000(t) / 86400.;
sunMeanAnom_d = fmod (MEAN_ECLIPTIC_LONGITUDE (ndays) - PERIGEE_LONGITUDE (ndays),
360.);
sunEclipLong_r = sunEclipLongitude (t);
moonLong_d = fmod (LUNAR_MEAN_LONGITUDE + (ndays * LUNAR_PROGRESSION),
360.);
/* 5: moon's mean anomaly */
moonMeanAnom_d = fmod ((moonLong_d - (0.1114041 * ndays)
- (LUNAR_PERIGEE_MEAN_LONG + LUNAR_NODE_MEAN_LONG)),
360.);
/* 6: ascending node mean longitude */
ascNodeMeanLong_d = fmod (LUNAR_NODE_MEAN_LONG - (0.0529539 * ndays),
360.);
eviction_d = 1.2739 /* 7: eviction */
* sin (DEGREES_TO_RADIANS (2.0 * (moonLong_d - RADIANS_TO_DEGREES (sunEclipLong_r))
- moonMeanAnom_d));
sinSunMeanAnom = sin (DEGREES_TO_RADIANS (sunMeanAnom_d));
Ae = 0.1858 * sinSunMeanAnom;
A3 = 0.37 * sinSunMeanAnom; /* 8: annual equation */
moonMeanAnom_d += eviction_d - Ae - A3; /* 9: "third correction" */
moonMeanAnom_r = DEGREES_TO_RADIANS (moonMeanAnom_d);
Ec = 6.2886 * sin (moonMeanAnom_r); /* 10: equation of center */
A4 = 0.214 * sin (2.0 * moonMeanAnom_r); /* 11: "yet another correction" */
/* Steps 12-14 give the true longitude after correcting for variation */
moonLong_d += eviction_d + Ec - Ae + A4
+ (0.6583 * sin (2.0 * (moonMeanAnom_r - sunEclipLong_r)));
/* 15: corrected longitude of node */
corrLong_d = ascNodeMeanLong_d - 0.16 * sinSunMeanAnom;
/*
* Calculate ecliptic latitude (16-19) and longitude (20) of the moon,
* then convert to right ascension and declination.
*/
lN_r = DEGREES_TO_RADIANS (moonLong_d - corrLong_d); /* l''-N' */
lambda_r = DEGREES_TO_RADIANS(corrLong_d)
+ atan2 (sin (lN_r) * cos (LUNAR_INCLINATION), cos (lN_r));
beta_r = asin (sin (lN_r) * sin (LUNAR_INCLINATION));
ecl2equ (t, lambda_r, beta_r, &ra_h, &decl_r);
/*
* The phase is the angle from the sun's longitude to the moon's
*/
*moonphase =
fmod (15.*ra_h - RADIANS_TO_DEGREES (sunEclipLongitude (update)),
360.);
if (*moonphase < 0)
*moonphase += 360;
*moonlatitude = RADIANS_TO_DEGREES (decl_r);
return TRUE;
}
