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