void RangeScanner::construct_laser_stripe(Vector3 _startPt, double _shift, double _laserFOV, LaserStripe& stripe) {
Vector3 laser_start = _startPt-laser_pos;
laser_start.normalize();
Vector3 baseline = sensor.camera_pos() - laser_pos;
baseline.normalize();
Vector3 rotation_axis = laser_start.cross(baseline);
rotation_axis.normalize();
// rotate laser start about rotation axis, by _shift amount and width _laserFOV
RotationMatrix rotation(rotation_axis, _shift);
Vector3 rotated_laser_centered = rotation.rotate(laser_start);
RotationMatrix minus_rotation(rotation_axis, _shift-0.5*_laserFOV);
Vector3 rotated_laser_minus = minus_rotation.rotate(laser_start);
RotationMatrix plus_rotation(rotation_axis, _shift+0.5*_laserFOV);
Vector3 rotated_laser_plus = plus_rotation.rotate(laser_start);
//cout << "laser stripe: " << rotated_laser_minus << " : " << rotated_laser_centered << " : " << rotated_laser_plus << endl;
// obtain laser stripe planes
//Vector3 minus_n = rotated_laser_minus.cross(rotation_axis);
Vector3 minus_n = rotation_axis.cross(rotated_laser_minus);
minus_n.normalize();
double minus_d = -(minus_n.dotProduct(laser_pos));
//Vector3 plus_n = rotation_axis.cross(rotated_laser_plus);
Vector3 plus_n = rotated_laser_plus.cross(rotation_axis);
plus_n.normalize();
double plus_d = -(plus_n.dotProduct(laser_pos));
Vector3 centered_n = rotated_laser_centered.cross(rotation_axis);
centered_n.normalize();
double centered_d = -(centered_n.dotProduct(laser_pos));
stripe = LaserStripe(minus_n, minus_d, centered_n, centered_d, plus_n, plus_d);
}
bool mHiObjectLoader::HandleMessage(const Telegram& msg)
{
if (MSG_SIZE <= msg.Msg)
{
return false;
}
if (HIOBJECT_LOADER_PITCH_MINUS == msg.Msg)
{
osg::Quat minus_rotation(PER_ANGLE * M_PI / 180, osg::Vec3(1.0f, 0.0f, 0.0));
osg::Matrixf rotation_matrix(minus_rotation);
transform_node_->postMult(rotation_matrix);
}else if (HIOBJECT_LOADER_PITCH_PLUS == msg.Msg)
{
osg::Quat minus_rotation(-PER_ANGLE * M_PI / 180, osg::Vec3(1.0f, 0.0f, 0.0));
osg::Matrixf rotation_matrix(minus_rotation);
transform_node_->postMult(rotation_matrix);
}else if (HIOBJECT_LOADER_ROLL_MINUS == msg.Msg)
{
osg::Quat minus_rotation(PER_ANGLE * M_PI / 180, osg::Vec3(0.0f, 1.0f, 0.0));
osg::Matrixf rotation_matrix(minus_rotation);
transform_node_->postMult(rotation_matrix);
}else if (HIOBJECT_LOADER_ROLL_PLUS == msg.Msg)
{
osg::Quat minus_rotation(-PER_ANGLE * M_PI / 180, osg::Vec3(0.0f, 1.0f, 0.0));
osg::Matrixf rotation_matrix(minus_rotation);
transform_node_->postMult(rotation_matrix);
}else if (HIOBJECT_LOADER_SCALE_MINUS == msg.Msg)
{
osg::Matrixf scale_matrix;
scale_matrix.makeScale(osg::Vec3(SCALE_MINUS_VALUE,
SCALE_MINUS_VALUE,
SCALE_MINUS_VALUE));
transform_node_->postMult(scale_matrix);
}else if (HIOBJECT_LOADER_SCALE_PLUS == msg.Msg)
{
osg::Matrixf scale_matrix;
scale_matrix.makeScale(osg::Vec3(SCALE_PLUS_VALUE,
SCALE_PLUS_VALUE,
SCALE_PLUS_VALUE));
transform_node_->postMult(scale_matrix);
}else if (HIOBJECT_LOADER_TRANSLATION_UP == msg.Msg)
{
osg::Matrixf translation_matrix;
translation_matrix.makeTranslate(osg::Vec3(0,
0,
TRANSLATION_VALUE));
transform_node_->postMult(translation_matrix);
}else if (HIOBJECT_LOADER_TRANSLATION_LEFT == msg.Msg)
{
osg::Matrixf translation_matrix;
translation_matrix.makeTranslate(osg::Vec3(-TRANSLATION_VALUE,
0,
0));
transform_node_->postMult(translation_matrix);
}else if (HIOBJECT_LOADER_TRANSLATION_RIGHT == msg.Msg)
{
osg::Matrixf translation_matrix;
translation_matrix.makeTranslate(osg::Vec3(TRANSLATION_VALUE,
0,
0));
transform_node_->postMult(translation_matrix);
}else if (HIOBJECT_LOADER_TRANSLATION_DOWN == msg.Msg)
{
osg::Matrixf translation_matrix;
translation_matrix.makeTranslate(osg::Vec3(0,
0,
-TRANSLATION_VALUE));
transform_node_->postMult(translation_matrix);
}else if (HIOBJECT_LOADER_YAW_MINUS == msg.Msg)
{
osg::Quat minus_rotation(-PER_ANGLE * M_PI / 180, osg::Vec3(0.0f, 0.0f, 1.0));
osg::Matrixf rotation_matrix(minus_rotation);
transform_node_->postMult(rotation_matrix);
}else if (HIOBJECT_LOADER_YAW_PLUS == msg.Msg)
{
osg::Quat minus_rotation(PER_ANGLE * M_PI / 180, osg::Vec3(0.0f, 0.0f, 1.0));
osg::Matrixf rotation_matrix(minus_rotation);
transform_node_->postMult(rotation_matrix);
}
return true;
}