void mat3::set_rotation(const math::vec3 &u, const math::vec3 &v)
{
scalar phi = u.dot_product(v);
vec3 w = u.cross_product(v);
scalar lambda = u.dot_product(u);
scalar h;
if (lambda > EPSILON)
h = (ONE - phi) / lambda;
else
h = lambda;
scalar hxy = w.x * w.y * h;
scalar hxz = w.x * w.z * h;
scalar hyz = w.y * w.z * h;
a00 = phi + w.x * w.x * h;
a01 = hxy - w.z;
a02 = hxz + w.y;
a10 = hxy + w.z;
a11 = phi + w.y * w.y * h;
a12 = hyz - w.x;
a20 = hxz - w.y;
a21 = hyz + w.x;
a22 = phi + w.z * w.z * h;
}
/// Distance between point p and line segment p1, p2
static double distance(math::vec3 const& p, math::vec3 const& p1, math::vec3 const& p2)
{
math::vec3 const v = p2 - p1;
math::vec3 const w = p - p1;
double const c1 = w.dot(v);
double const c2 = v.dot(v);
double const b = c1 / c2;
math::vec3 const pb = p1 + v * b;
return p.distance(pb);
}
static bool segment_plane_intersect(math::vec3 const& segment_point1, math::vec3 const& segment_point2,
math::vec3 const& plane_p0, math::vec3 const& plane_n, math::vec3& result)
{
math::vec3 const dir = (segment_point2 - segment_point1).normalize(); // ray direction
double const denom = plane_n.dot(dir);
if (math::is_zero(denom))
{
return false; // no intersection: ray is parallel to plane
}
math::vec3 const p1 = plane_p0 - segment_point1;
double const d = p1.dot(plane_n) / denom;
result = segment_point1 + dir * d; // intersection point
return true;
}
// sphere/sphere
hit::list sphere::collide(const sphere& p) const {
hit::list res;
const math::vec3 diff = p.geo.center - geo.center;
const math::real dist = diff.norm();
// core::log("dist:", dist, "r1:", geo.radius, "r2:", p.geo.radius);
const math::real error = p.geo.radius + geo.radius - dist;
if( dist && (error >= 0) ) {
math::real alpha = 0.5 * (geo.radius + dist - p.geo.radius);
assert( alpha >= 0 );
const math::vec3 n = diff / dist;
const math::vec3 pos = geo.center + alpha * n;
res.push_back( hit{this, &p, pos, n, error} );
}
return res;
}
void BoundingPolyhedron::positionAround(const math::vec3 desiredCentre, const std::vector<math::vec3>& points)
{
if(!mInitialised)
throw::std::runtime_error("Using uninitialised bounding polyhedron");
if(points.size()==0)
return;
auto bounding = calculateBoundSphere(points);
float radius = (bounding.second + desiredCentre.distance(bounding.first))*mDesc.scaleMultiplier;
setCentreAndRadius(desiredCentre, radius);
}
static bool segment_sphere_intersect(math::vec3 const& segment_point1, math::vec3 const& segment_point2,
math::vec3 const& sphere_center, double sphere_radius)
{
#if 0
math::vec3 closest_point;
closest_point.get_nearest_point_on_line(sphere_center, segment_point1, segment_point2);
math::vec3 const distance = closest_point - sphere_center;
double const sqr_distance = distance.dot(distance);
double const sqr_radius = sphere_radius * sphere_radius;
return sqr_distance <= sqr_radius;
#else
math::vec3 const p = segment_point1 - sphere_center; // ray_origin - sphere_center
math::vec3 const d = (segment_point2 - segment_point1).normalize(); // ray_direction
double const a = d.dot(d);
double const b = 2 * d.dot(p);
double const c = p.dot(p) - sphere_radius * sphere_radius;
double const D = b * b - 4 * a * c;
if (D >= 0)
{
/*
intersection points:
double t[2];
t[0] = (-b - sqrt(D)) / (2 * a);
t[1] = (-b + sqrt(D)) / (2 * a);
math::vec3 result[2];
result[0] = segment_point1 + d * t[0];
result[1] = segment_point1 + d * t[1];
*/
return true;
}
return false;
#endif
}
cv::Mat AlphaRayLocator::findAlphas(
const BoundingPolyhedron* polyhedrons,
const size_t polyhedronCount,
const ia::InputAssembler& input) const
{
assert(polyhedronCount>1);
START_TIMER(AlphaLocator);
const cv::Mat& mat = input.mat();
const unsigned r = input.mat().rows, c = input.mat().cols;
const math::vec3 background = input.background();
cv::Mat out;
out.create(r, c, CV_32FC1);
const SpherePolyhedron& outerPoly = polyhedrons[1];
const SpherePolyhedron& innerPoly = polyhedrons[0];
//For each point, send rays
for (unsigned i = 0; i < r; ++i)
{
float* data = (float*)(mat.data + mat.step*i);
float* dataOut = (float*)(out.data + out.step*i);
for(unsigned j = 0; j < c; ++j)
{
math::vec3& point = *((math::vec3*)(data + j*3));
float alpha;
//If right in the middle
if(background==point)
alpha = 0;
else
{
//Prepare vector
const math::vec3 vector = point - background;
const float vectorLen = vector.length();
const math::vec3 vectorNorm = vector/vectorLen;
const float distanceToPoint = point.distance(background);
const float distanceToOuterPoly = outerPoly.findDistanceToPolyhedron(vectorNorm);
//If inside outer poly, alpha < 1
if(distanceToPoint < distanceToOuterPoly)
{
float distanceToInnerPoly = innerPoly.findDistanceToPolyhedron(vectorNorm);
//If does not intersect with inner, fully inside
if(distanceToPoint < distanceToInnerPoly)
alpha = 0;
else //interpolate between inner and outer
alpha = (vectorLen - distanceToInnerPoly) /
(distanceToOuterPoly - distanceToInnerPoly);
}
else //If intersects with middle, it's outside. Alpha = 1.
alpha = 1;
}
*(dataOut+j) = alpha;
}
}
END_TIMER(AlphaLocator);
return out;
}
void TerrainObserver::SetPosition(const Math::vec3& value)
{
m_view->UpdatePosition(value.XZ());
}
void BulletSimulator::SetGravity(const Math::vec3& g)
{
btVector3 v(g.X(), g.Y(), g.Z());
m_dynamics_world->setGravity(v);
}
void Shader::setUniform3f(const QString &name, math::vec3<float> &vector)
{
oglfunc.glUniform3f(getUniformLocation((GLchar *)name.toStdString().c_str()),vector.getX(),vector.getY(),vector.getZ());
}
/** \brief Expand the boundary.
*
* Uses some manual checking to decrease comparison count instead of pure
* min/max.
*
* \param pnt Point to expand with.
*/
void expand(const math::vec3<T> &pnt)
{
if(pnt.x() > m_pnt_max.x())
{
m_pnt_max.x() = pnt.x();
}
else if(pnt.x() < m_pnt_min.x())
{
m_pnt_min.x() = pnt.x();
}
if(pnt.y() > m_pnt_max.y())
{
m_pnt_max.y() = pnt.y();
}
else if(pnt.y() < m_pnt_min.y())
{
m_pnt_min.y() = pnt.y();
}
if(pnt.z() > m_pnt_max.z())
{
m_pnt_max.z() = pnt.z();
}
else if(pnt.z() < m_pnt_min.z())
{
m_pnt_min.z() = pnt.z();
}
}