void GLGameModel::pointInDirection(QVector3D d)
{
if(d.isNull())
{
setAngleY(0.0);
return;
}
// roate the object to point in direction
// we only rotate in the x-z plane
float cos_y = d.normalized().z();
setAngleY(360.0 * qAcos(cos_y) / (2*M_PI));
//qDebug() << "pointing to" << d << "with angles" << angleX << angleY << angleZ;
}
VisualCorner::VisualCorner(const int _x, const int _y,
const float _distance,
const float _bearing,
boost::shared_ptr<VisualLine> l1, boost::shared_ptr<VisualLine> l2,
const float _t1, const float _t2,
boost::shared_ptr<NaoPose> _pose)
: VisualObject(CORNER_NO_IDEA_ID,_x, _y, _distance, _bearing),
pose(_pose),
possibleCorners(ConcreteCorner::concreteCorners().begin(),
ConcreteCorner::concreteCorners().end()),
cornerType(UNKNOWN), secondaryShape(UNKNOWN), line1(l1), line2(l2),
lines(), t1(_t1), t2(_t2),
// Technically the initialization of tBar and tStem is incorrect here for
// which we apologize. It's a hack, but the true values of tBar and tStem
// will get assigned in determineCornerShape which is right here in the
// constructor.
tBar(line1), tStem(line2),
angleBetweenLines(0), orientation(0), physicalOrientation(0)
{
lines.push_back(line1);
lines.push_back(line2);
determineCornerShape();
// Calculate and set the standard deviation of the measurements
setDistanceSD(cornerDistanceToSD(_distance));
setBearingSD(cornerBearingToSD(_bearing, _distance));
setAngleX( static_cast<float>(HALF_IMAGE_WIDTH - _x) /
static_cast<float>(HALF_IMAGE_WIDTH) *
MAX_BEARING_RAD);
setAngleY(static_cast<float>(HALF_IMAGE_HEIGHT - _y) /
static_cast<float>(HALF_IMAGE_HEIGHT) *
MAX_ELEVATION_RAD);
}
