static bool line_aabb_intersection(struct AABB aabb, vec2 line_start, vec2 line_end)
{
// TODO: optimize?
return (line_line_intersection(line_start, line_end, vec2_new(aabb.min.x, aabb.min.y), vec2_new(aabb.max.x, aabb.min.y)) ||
line_line_intersection(line_start, line_end, vec2_new(aabb.max.x, aabb.min.y), vec2_new(aabb.max.x, aabb.max.y)) ||
line_line_intersection(line_start, line_end, vec2_new(aabb.max.x, aabb.max.y), vec2_new(aabb.min.x, aabb.max.y)) ||
line_line_intersection(line_start, line_end, vec2_new(aabb.min.x, aabb.max.y), vec2_new(aabb.min.x, aabb.min.y)));
}
Ballistics::Ballistics(cv::Mat p1, cv::Mat p2, double alpha, cv::Mat q1, cv::Mat q2) {
_center = line_line_intersection(p1, p2, q1, q2);
/* old_front is the orthonormal vector
* that pointed to the front view of the laser,
* in the first two measurements.
*
* Mathematically speaking, there are two possible ways of computing old_front:
* normalizing p1 - _center, and normalizing p2 - _center.
* In real life, p1 and p2 almost certainly will be given to us with errors.
* We hope to minimize the error by picking the largest of
* (p1 - _center) and (p2 - _center) to normalize.
* (That is, the error relative to _center will probably be smaller
* if we pick the vector that is more distant.)
*/
cv::Mat old_front = p1 - _center;
if( cv::norm(p2 - _center) > cv::norm(old_front) )
old_front = p2 - _center;
// And now normalize.
old_front /= cv::norm(old_front);
// Repeating the same trick for computing _front
_front = q1 - _center;
if( cv::norm(q2 - _center) > cv::norm(_front) )
_front = q2 - _center;
_front /= cv::norm(_front);
double actual_angle = angle_between( old_front, _front );
double projection_angle = angle_of_projection( actual_angle, alpha );
if( alpha > 0 ) {
/* There was a positive rotation from old_front to _front,
* and it is, by exigence, less than pi.
* So, this satisfies the assumptions of compute_rotation_axis.
*/
_up = compute_rotation_axis( old_front, _front, projection_angle );
}
else {
/* There was a negative rotation from old_front to _front.
* This violetes the assumptions made by compute_rotation_axis,
* so we will pretend that there was a positive rotation
* from _front to old_front during the computation of _up.
*/
_up = compute_rotation_axis( _front, old_front, projection_angle );
}
_left = _up.cross(_front);
_up /= cv::norm(_up);
_left /= cv::norm(_left);
}
