bool fi::VPDetectionWrapper::getRansacInliers(const std::vector<Eigen::Vector3f> &vp_hypothesis, std::vector<Eigen::Vector3f> &in_liers, float angular_tolerance)
{
unsigned int num_vps = vp_hypothesis.size();
unsigned int max_count = 0;
unsigned int best_index = -1;
for ( unsigned int i = 0; i < num_vps; i++)
{
Eigen::Vector3f tmp_n = vp_hypothesis.at(i);
unsigned int tmp_count = 0;
for (unsigned int j = 0; j < num_vps; j++)
{
float angle_value_radiens = angleBetweenVectors(tmp_n, vp_hypothesis.at(j));
float angle_value_degrees = pcl::rad2deg(angle_value_radiens);
if (angle_value_degrees < angular_tolerance)
{
tmp_count++;
}
}
if (tmp_count > max_count)
{
max_count = tmp_count;
best_index = i;
}
}
//collect inliers
Eigen::Vector3f best_querry = vp_hypothesis.at(best_index);
for (unsigned int j = 0; j < num_vps; j++)
{
float angle_value_radiens = angleBetweenVectors(best_querry, vp_hypothesis.at(j));
float angle_value_degrees = pcl::rad2deg(angle_value_radiens);
if (angle_value_degrees < angular_tolerance)
{
in_liers.push_back(vp_hypothesis.at(j));
}
}
if (in_liers.size() == 0)
{
return false;
}
return true;
}
void
SoXipOverlayTransformBoxManip::rotate( SoHandleEventAction* action )
{
SbVec3f projPt;
getPoint( action, projPt );
const SbVec3f* point = mControlPointsCoords->point.getValues(0);
SbVec3f center = (point[0] + point[4]) / 2.;
SbVec3f rotateFrom = point[mControlPointId] - center;
SbVec3f rotateTo = projPt - center;
SbVec3f normal = mViewVolume.getPlane(0).getNormal();
SbRotation rotation;
rotation.setValue( normal, angleBetweenVectors( rotateFrom, rotateTo, normal ) );
SbMatrix centerMatrix;
centerMatrix.setTranslate( center );
SbMatrix rotationMatrix;
rotationMatrix.setRotate( rotation );
mTransformationMatrix = centerMatrix.inverse() * rotationMatrix * centerMatrix;
transform( mActionNode );
}
void
SoXipAnnotation::updateEnumerationPosition( SoSFVec3f& position )
{
if( mIsViewInitialized && point.getNum() >= 2 )
{
const SbVec3f& p0 = point[0];
const SbVec3f& p1 = point[1];
SbVec3f u = SbVec3f( 1, 0, 0 );
SbVec3f v = p1 - p0;
SbVec3f normal = mViewVolume.getPlane(0).getNormal();
double a = angleBetweenVectors( u, v, normal );
bool pos_cos = cos(a) > 0;
bool pos_sin = sin(a) > 0;
mEnumerationText->justification.setValue( pos_cos ? SoXipText2::LEFT : SoXipText2::RIGHT );
mEnumerationText->vAlignment.setValue( pos_sin ? SoXipText2::TOP : SoXipText2::BOTTOM );
SbVec3f offset = projectScreenOffsetToWorld( SbVec2s( 5, 5 ) );
SbVec3f middle;
middle[0] = ( pos_cos ? offset[0] : -offset[0] ) + ( p0[0] + p1[0] ) / 2.;
middle[1] = ( !pos_sin ? offset[1] : -offset[1] ) + ( p0[1] + p1[1] ) / 2.;
middle[2] = ( p0[2] + p1[2] ) / 2.;
position.setValue( middle );
}
}
void FrictionUtilities::generateOrthogonalVectors( const Vector3s& n, std::vector<Vector3s>& vectors, const Vector3s& suggested_tangent )
{
if( vectors.empty() )
{
return;
}
assert( ( suggested_tangent.cross( n ) ).squaredNorm() != 0.0 );
// Make sure the first vector is orthogonal to n and unit length
vectors[0] = ( suggested_tangent - n.dot( suggested_tangent ) * n ).normalized();
assert( fabs( vectors[0].norm() - 1.0 ) <= 1.0e-10 );
assert( fabs( n.dot( vectors[0] ) ) <= 1.0e-10 );
// Generate the remaining vectors by rotating the first vector about n
const scalar dtheta{ 2.0 * MathDefines::PI<scalar>() / scalar( vectors.size() ) };
for( std::vector<Vector3s>::size_type i = 1; i < vectors.size(); ++i )
{
vectors[i] = Eigen::AngleAxis<scalar>{ i * dtheta, n } * vectors[0];
assert( fabs( vectors[i].norm() - 1.0 ) <= 1.0e-10 );
assert( fabs( n.dot( vectors[i] ) ) <= 1.0e-10 );
}
#ifndef NDEBUG
if( vectors.size() == 1 )
{
return;
}
// Check that the angle between each vector is the one we expect
for( std::vector<Vector3s>::size_type i = 0; i < vectors.size(); ++i )
{
assert( fabs( angleBetweenVectors( vectors[i], vectors[( i + 1 ) % vectors.size()] ) - dtheta ) < 1.0e-6 );
}
#endif
}
void ItemElement::setDraggedRotation(QPointF pos, QPointF lastPos)
{
QVector2D vec(pos-boundingRect().center());
QVector2D lastVec(lastPos-boundingRect().center());
float ang = angleBetweenVectors(vec, lastVec);
setRotation(rotation() + ang);
}
bool Hunter::monsterSpotted(Vector2 player)
{
double angleBetweenChars = angleBetweenVectors(this->body->getPosition(), player);
double angleAmount = abs(this->body->getDirection() - angleBetweenChars);
if (angleAmount <= halfAngle && distanceBetweenVectors(this->body->getPosition(), player) <= spotRange)
{
return true;
}
return false;
}
void
SoXipAnnotation::GLRender( SoGLRenderAction* action )
{
if( mIsViewInitialized && point.getNum() == 2 && point[0] != point[1] )
{
float angleValue = angle.getValue() * M_PI / 180.;
SbVec3f screenPt[2];
mViewVolume.projectToScreen( point[0], screenPt[0] );
mViewVolume.projectToScreen( point[1], screenPt[1] );
float lineAngle = angleBetweenVectors( SbVec3f(1, 0, 0), (screenPt[1] - screenPt[0]) );
if( screenPt[0][1] > screenPt[1][1] )
lineAngle = 2 * M_PI - lineAngle;
float arrowLengthPix = length.getValue();
SbVec3f offsetUp = projectScreenOffsetToWorld( SbVec2s(
arrowLengthPix * cos( lineAngle - angleValue ),
arrowLengthPix * sin( lineAngle - angleValue ) ) );
SbVec3f offsetDown = projectScreenOffsetToWorld( SbVec2s(
arrowLengthPix * cos( lineAngle + angleValue ),
arrowLengthPix * sin( lineAngle + angleValue ) ) );
mArrowCoords->point.setNum(3);
mArrowCoords->point.set1Value( 0, point[0] );
mArrowCoords->point.set1Value( 1, point[0] + offsetUp );
mArrowCoords->point.set1Value( 2, point[0] + offsetDown );
int indices[6] = {0, 1, -1, 0, 2, -1};
mArrowIndices->coordIndex.setNum(6);
mArrowIndices->coordIndex.setValues( 0, 6, indices );
}
SoXipManipulableShape::GLRender( action );
}
void Checker::adjustPolarAngleRanges (const DocumentModelCoords &modelCoords,
const Transformation &transformation,
const QList<Point> &points,
double &xMin,
double &xMax,
double &yMin) const
{
LOG4CPP_INFO_S ((*mainCat)) << "Checker::adjustPolarAngleRanges transformation=" << transformation;
const double UNIT_LENGTH = 1.0;
QString path; // For logging
if (modelCoords.coordsType() == COORDS_TYPE_POLAR) {
// Range minimum is at origin
yMin = modelCoords.originRadius();
path = QString ("yMin=%1 ").arg (yMin); // For logging
// Perform special processing to account for periodicity of polar coordinates. Start with unit vectors
// in the directions of the three axis points
double angle0 = points.at(0).posGraph().x();
double angle1 = points.at(1).posGraph().x();
double angle2 = points.at(2).posGraph().x();
QPointF pos0 = transformation.cartesianFromCartesianOrPolar(modelCoords,
QPointF (angle0, UNIT_LENGTH));
QPointF pos1 = transformation.cartesianFromCartesianOrPolar(modelCoords,
QPointF (angle1, UNIT_LENGTH));
QPointF pos2 = transformation.cartesianFromCartesianOrPolar(modelCoords,
QPointF (angle2, UNIT_LENGTH));
// Identify the axis point that is more in the center of the other two axis points. The arc is then drawn
// from one of the other two points to the other. Center point has smaller angles with the other points
double sumAngle0 = angleBetweenVectors(pos0, pos1) + angleBetweenVectors(pos0, pos2);
double sumAngle1 = angleBetweenVectors(pos1, pos0) + angleBetweenVectors(pos1, pos2);
double sumAngle2 = angleBetweenVectors(pos2, pos0) + angleBetweenVectors(pos2, pos1);
if ((sumAngle0 <= sumAngle1) && (sumAngle0 <= sumAngle2)) {
// Point 0 is in the middle. Either or neither of points 1 and 2 may be along point 0
if ((angleFromVectorToVector (pos0, pos1) < 0) ||
(angleFromVectorToVector (pos0, pos2) > 0)) {
path += QString ("from 1=%1 through 0 to 2=%2").arg (angle1).arg (angle2);
xMin = angle1;
xMax = angle2;
} else {
path += QString ("from 2=%1 through 0 to 1=%2").arg (angle2).arg (angle1);
xMin = angle2;
xMax = angle1;
}
} else if ((sumAngle1 <= sumAngle0) && (sumAngle1 <= sumAngle2)) {
// Point 1 is in the middle. Either or neither of points 0 and 2 may be along point 1
if ((angleFromVectorToVector (pos1, pos0) < 0) ||
(angleFromVectorToVector (pos1, pos2) > 0)) {
path += QString ("from 0=%1 through 1 to 2=%2").arg (angle0).arg (angle2);
xMin = angle0;
xMax = angle2;
} else {
path += QString ("from 2=%1 through 1 to 0=%2").arg (angle2).arg (angle0);
xMin = angle2;
xMax = angle0;
}
} else {
// Point 2 is in the middle. Either or neither of points 0 and 1 may be along point 2
if ((angleFromVectorToVector (pos2, pos0) < 0) ||
(angleFromVectorToVector (pos2, pos1) > 0)) {
path += QString ("from 0=%1 through 2 to 1=%2").arg (angle0).arg (angle1);
xMin = angle0;
xMax = angle1;
} else {
path += QString ("from 1=%1 through 2 to 0=%2").arg (angle1).arg (angle0);
xMin = angle1;
xMax = angle0;
}
}
// Make sure theta is increasing
while (xMax < xMin) {
double thetaPeriod = modelCoords.thetaPeriod();
path += QString (" xMax+=%1").arg (thetaPeriod);
xMax += thetaPeriod;
}
}
LOG4CPP_INFO_S ((*mainCat)) << "Checker::adjustPolarAngleRanges path=(" << path.toLatin1().data() << ")";
}
LinearAnimation::LinearAnimation(float span, vector<float> controlPoints): Animation(){
this->totalSpan = span * 1000;
this->controlPoints = controlPoints;
this->numTrajectories = (this->controlPoints.size() / 3) - 1;
this->totalDist = 0;
//Load trajectory distances, calculate total distance and coordinate deltas between trajectories
for(unsigned int i = 0; i < numTrajectories; i++) {
//Get this point and the next
float point1[3], point2[3];
int ind1 = (i * 3);
int ind2 = ((i+1) * 3);
point1[0] = controlPoints.at(0 + ind1);
point1[1] = controlPoints.at(1 + ind1);
point1[2] = controlPoints.at(2 + ind1);
point2[0] = controlPoints.at(0 + ind2);
point2[1] = controlPoints.at(1 + ind2);
point2[2] = controlPoints.at(2 + ind2);
//Calculate distance between those 2 points and add it to total distance and trajectory distances vector
float trajDist = distanceBetweenPoints(point1, point2);
totalDist += trajDist;
trajectoryDists.push_back(trajDist);
//Calculate deltas between coordinates of the two points and add them to deltas vector
float deltaX;
float deltaY;
float deltaZ;
deltaX = point2[0] - point1[0];
deltaY = point2[1] - point1[1];
deltaZ = point2[2] - point1[2];
trajectoryCoordDeltas.push_back(deltaX);
trajectoryCoordDeltas.push_back(deltaY);
trajectoryCoordDeltas.push_back(deltaZ);
//Calculate the previous offsets for the trajectory coordinates (that is, the sum of the previous deltas), and push them to the vector
int previousIndex;
if(i == 0) {
trajectoryCoordPreviousOffsets.push_back(0);
trajectoryCoordPreviousOffsets.push_back(0);
trajectoryCoordPreviousOffsets.push_back(0);
previousIndex = 0;
}
else
previousIndex = i*3;
if(i < (numTrajectories - 1) ) {
trajectoryCoordPreviousOffsets.push_back(deltaX + trajectoryCoordPreviousOffsets.at(previousIndex + 0));
trajectoryCoordPreviousOffsets.push_back(deltaY + trajectoryCoordPreviousOffsets.at(previousIndex + 1));
trajectoryCoordPreviousOffsets.push_back(deltaZ + trajectoryCoordPreviousOffsets.at(previousIndex + 2));
}
}
//Calculate angles between the trajectories based on the trajectory vector components on the XZ plane, always summing the angle with the previous one
for(unsigned int i = 0; i < numTrajectories - 1; i++) {
float previousAngle;
if(i == 0)
previousAngle = 0;
else
previousAngle = trajectoryAngles.at(i - 1);
int ind = i * 3;
int ind2 = i * 3 + 3;
float vector1[3];
vector1[0] = trajectoryCoordDeltas.at(ind + 0);
//Since we want the vector projections in the XZ plane, we consider y = 0
vector1[1] = 0;
vector1[2] = trajectoryCoordDeltas.at(ind + 2);
float vector2[3];
vector2[0] = trajectoryCoordDeltas.at(ind2 + 0);
//Since we want the vector projections in the XZ plane, we consider y = 0
vector2[1] = 0;
vector2[2] = trajectoryCoordDeltas.at(ind2 + 2);
float rotAngle = angleBetweenVectors(vector2, vector1);
//To ensure we use angles between 0 and 360, for simplicity
if(rotAngle > 360)
rotAngle -= 360;
else if(rotAngle < 0)
rotAngle += 360;
rotAngle += previousAngle;
trajectoryAngles.push_back(rotAngle);
}
// Using the trajectory distances and the total distance, calculate the fraction of total animation
//that each trajectory represents, and thus calculate the different time spans each trajectory
//shall need to complete its animation.
for(unsigned int i = 0; i < numTrajectories; i++) {
float distFraction = trajectoryDists.at(i) / totalDist;
unsigned long timeSpan = (distFraction * totalSpan);
timeSpans.push_back(timeSpan);
}
currentTrajectory = 0;
elapsedTimeInTraj = 0;
totalElapsedTime = 0;
ended = false;
}
void fi::VPDetectionWrapper::validateVanishingPoint(const std::vector<std::vector< Eigen::Vector2f> > &computed_vp_hypothesis, const Eigen::Matrix3f &cam_calib, Eigen::Vector3f &final_robust_vp_x, Eigen::Vector3f &final_robust_vp_y)
{
Eigen::Matrix3f inv_cam_calib = cam_calib.inverse();
//trans from vps to rays through camera axis, see Z+H Chapter 8, more on single view geometry!
unsigned int num_vps = computed_vp_hypothesis.size();
std::vector< Eigen::Vector3f> computed_vp_hypothesis_x;
std::vector< Eigen::Vector3f> computed_vp_hypothesis_y;
std::vector< Eigen::Vector3f> computed_vp_hypothesis_z;
for (unsigned int i = 0; i < num_vps; i++)
{
std::vector<Eigen::Vector2f> a_vp = computed_vp_hypothesis.at(i);
Eigen::Vector2f a_x = a_vp.at(0);
Eigen::Vector3f x_h, n_x;
x_h(0) = a_x(0);
x_h(1) = a_x(1);
x_h(2) = 1;
n_x = inv_cam_calib * x_h;
n_x = n_x.normalized();
computed_vp_hypothesis_x.push_back(n_x);
Eigen::Vector2f a_y = a_vp.at(1);
Eigen::Vector3f y_h, n_y;
y_h(0) = a_y(0);
y_h(1) = a_y(1);
y_h(2) = 1;
n_y = inv_cam_calib * y_h;
n_y = n_y.normalized();
computed_vp_hypothesis_y.push_back(n_y);
Eigen::Vector2f a_z = a_vp.at(2);
Eigen::Vector3f z_h, n_z;
z_h(0) = a_z(0);
z_h(1) = a_z(1);
z_h(2) = 1;
n_z = inv_cam_calib * z_h;
n_z = n_z.normalized();
computed_vp_hypothesis_z.push_back(n_z);
}
std::vector<Eigen::Vector3f> in_liers_x;
std::vector<Eigen::Vector3f> in_liers_y;
std::vector<Eigen::Vector3f> in_liers_z;
bool found_inliers_x = getRansacInliers(computed_vp_hypothesis_x, in_liers_x);
bool found_inliers_y = getRansacInliers(computed_vp_hypothesis_y, in_liers_y);
bool found_inliers_z = getRansacInliers(computed_vp_hypothesis_z, in_liers_z);
Eigen::VectorXf optimized_vp_x;
Eigen::VectorXf optimized_vp_y;
Eigen::VectorXf optimized_vp_z;
leastQuaresVPFitting(in_liers_x, optimized_vp_x);
leastQuaresVPFitting(in_liers_y, optimized_vp_y);
leastQuaresVPFitting(in_liers_z, optimized_vp_z);
std::cout<<"Vanishing Points Validated"<<std::endl;
//test the angles and see if OK otherwise check again if truelly orthogonal
Eigen::Vector3f vp_x (optimized_vp_x[3], optimized_vp_x[4], optimized_vp_x[5]);;
Eigen::Vector3f vp_y (optimized_vp_y[3], optimized_vp_y[4], optimized_vp_y[5]);
Eigen::Vector3f vp_z (optimized_vp_z[3], optimized_vp_z[4], optimized_vp_z[5]);
Eigen::Vector3f vp_x_centroid (optimized_vp_x[0], optimized_vp_x[1], optimized_vp_x[2]);
Eigen::Vector3f vp_y_centroid (optimized_vp_y[0], optimized_vp_y[1], optimized_vp_y[2]);
Eigen::Vector3f vp_z_centroid (optimized_vp_z[0], optimized_vp_z[1], optimized_vp_z[2]);
float angle_value_radiens_cxy = angleBetweenVectors(vp_x_centroid, vp_y_centroid);
float angle_value_degrees_cxy = pcl::rad2deg(angle_value_radiens_cxy);
float angle_value_radiens_cxz = angleBetweenVectors(vp_x_centroid, vp_z_centroid);
float angle_value_degrees_cxz = pcl::rad2deg(angle_value_radiens_cxz);
float angle_value_radiens_cyz = angleBetweenVectors(vp_y_centroid, vp_z_centroid);
float angle_value_degrees_cyz = pcl::rad2deg(angle_value_radiens_cyz);
float angle_value_radiens_xy = angleBetweenVectors(vp_x, vp_y);
float angle_value_degrees_xy = pcl::rad2deg(angle_value_radiens_xy);
float angle_value_radiens_xz = angleBetweenVectors(vp_x, vp_z);
float angle_value_degrees_xz = pcl::rad2deg(angle_value_radiens_xz);
float angle_value_radiens_yz = angleBetweenVectors(vp_y, vp_z);
float angle_value_degrees_yz = pcl::rad2deg(angle_value_radiens_yz);
//collect only the mean vps
final_robust_vp_x = optimized_vp_x.tail<3> ();
final_robust_vp_y = optimized_vp_y.tail<3> ();
//final_robust_vp_x = vp_x_centroid;
//final_robust_vp_y = vp_y_centroid;
}
void Hunter::update(SDL_Rect playerLocation, long currentRunTime, int mapWidth, int mapHeight, std::vector<EnvironmentObject> obstacles)
{
previousLocation.x = this->body->getPosition().x + cos((this->body->getDirection() - 180) * PI / 180) * this->body->getSpeed();
previousLocation.y = this->body->getPosition().y + sin((this->body->getDirection() - 180) * PI / 180) * this->body->getSpeed();
std::mt19937 random;
random.seed(SDL_GetTicks());
std::uniform_int_distribution<int> randomDegree(0, 359);
Vector2 player = { (double)playerLocation.x , (double)playerLocation.y };
if (this->status != DEAD)
{
switch (action)
{
case HUNT:
if (currentRunTime >= directionChangeTime) {
int degree = randomDegree(random);
changeDirection(degree);
directionChangeTime = currentRunTime + turnInterval;
}
this->body->accelerate(this->body->getMaxSpeed());
break;
case TRACK:
this->body->decelerate(this->body->getMaxSpeed());
changeDirection((int)angleBetweenVectors(this->body->getPosition(), player));
if (distanceBetweenVectors(this->body->getPosition(), player) > spotRange)
{
action = CHECK;
lastPlayerLocation = player;
}
else if (distanceBetweenVectors(this->body->getPosition(), player) <= shootRange)
{
this->body->decelerate(this->body->getMaxSpeed());
action = SHOOT;
}
else
{
this->body->accelerate(this->body->getMaxSpeed());
}
break;
case CHECK:
changeDirection((int)angleBetweenVectors(this->body->getPosition(), lastPlayerLocation));
if (distanceBetweenVectors(this->body->getPosition(), lastPlayerLocation) > 5)
{
this->body->accelerate(this->body->getMaxSpeed());
}
else
{
this->body->decelerate(this->body->getMaxSpeed());
action = HUNT;
}
break;
case SHOOT:
status = SHOOTING;
}
if (this->body->getPosition().x < 0 || this->body->getPosition().x >= mapWidth)
{
this->body->changeDirection(this->body->getDirection() + 180);
}
if (this->body->getPosition().y < 0 || this->body->getPosition().y >= mapHeight)
{
this->body->changeDirection(this->body->getDirection() + 180);
}
for (int i = 0; i < obstacles.size(); i++) {
if (AABBCollision(obstacles[i].getCollisionBox(),this->objectCollison))
{
this->body->manuallyUpdatePosition(previousLocation);
this->body->decelerate(this->body->getMaxSpeed());
this->body->changeDirection(this->body->getDirection() + 180);
action = HUNT;
}
}
this->body->updateDisplacement(0, 60);
this->initCollision();
if (monsterSpotted(player) && action != FLEE && action != SHOOT) {
action = TRACK;
}
}
else
{
this->destroy();
}
}
double angleBetweenLinePlane(const Line& l, const Plane& p)
{
Vector v = p.normal(), u = l.getVector();
return (PI/2) - angleBetweenVectors(v, u);
}
