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); }