/* ************************************************************************* */ Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { double d = 1.0 / P.z(), d2 = d*d; const Cal3_S2Stereo& K = *K_; double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); return Matrix_(3, 3, f_x*d, 0.0, -d2*f_x* P.x(), f_x*d, 0.0, -d2*f_x*(P.x() - b), 0.0, f_y*d, -d2*f_y* P.y() ); }
void Camera::lookAt (const Point3 &eyePos, const Point3 ¢erOfView, const Vector3 &_up) { viewMatrix = Matrix4::Identity(); Vector3 up = _up; Vector3 direction; direction.x() = centerOfView.x() - eyePos.x(); direction.y() = centerOfView.y() - eyePos.y(); direction.z() = centerOfView.z() - eyePos.z(); direction.normalize(); up.normalize(); Vector3 side = direction.cross(up); Vector3 Utrue = side.cross (direction); viewMatrix (0, 0) = side.x(); viewMatrix (0, 1) = side.y(); viewMatrix (0, 2) = side.z(); viewMatrix (1, 0) = Utrue.x(); viewMatrix (1, 1) = Utrue.y(); viewMatrix (1, 2) = Utrue.z(); viewMatrix (2, 0) = -direction.x(); viewMatrix (2, 1) = -direction.y(); viewMatrix (2, 2) = -direction.z(); viewMatrix (0, 3) = -eyePos.x(); viewMatrix (1, 3) = -eyePos.y(); viewMatrix (2, 3) = -eyePos.z(); std::cout << viewMatrix << std::endl; }
/* ************************************************************************* */ StereoPoint2 StereoCamera::project(const Point3& point, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { #ifdef STEREOCAMERA_CHAIN_RULE const Point3 q = leftCamPose_.transform_to(point, H1, H2); #else // omit derivatives const Point3 q = leftCamPose_.transform_to(point); #endif if ( q.z() <= 0 ) throw StereoCheiralityException(); // get calibration const Cal3_S2Stereo& K = *K_; const double fx = K.fx(), fy = K.fy(), b = K.baseline(); // calculate scaled but not translated image coordinates const double d = 1.0 / q.z(); const double x = q.x(), y = q.y(); const double dfx = d*fx, dfy = d*fy; const double uL = dfx*x; const double uR = dfx*(x - b); const double v = dfy*y; // check if derivatives need to be computed if (H1 || H2) { #ifdef STEREOCAMERA_CHAIN_RULE // just implement chain rule Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian if (H1) *H1 = D_project_point*(*H1); if (H2) *H2 = D_project_point*(*H2); #else // optimized version, see StereoCamera.nb if (H1) { const double v1 = v/fy, v2 = fx*v1, dx=d*x; *H1 = (Matrix(3, 6) << uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL, uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR, fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v ); } if (H2) { const Matrix R(leftCamPose_.rotation().matrix()); *H2 = d * (Matrix(3, 3) << fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v ); } #endif } // finally translate return StereoPoint2(K.px() + uL, K.px() + uR, K.py() + v); }
/* ************************************************************************* */ TEST( dataSet, gtsam2openGL) { Vector3 rotVec(0.2, 0.7, 1.1); Rot3 R = Rot3::Expmap(rotVec); Point3 t = Point3(1.0,20.0,10.0); Pose3 actual = Pose3(R,t); Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z()); Pose3 expected = gtsam2openGL(poseGTSAM); EXPECT(assert_equal(expected,actual)); }
/* ************************************************************************* */ double PoseRTV::range(const PoseRTV& other, OptionalJacobian<1,9> H1, OptionalJacobian<1,9> H2) const { Matrix36 D_t1_pose, D_t2_other; const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0); const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0); Matrix13 D_d_t1, D_d_t2; double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0; if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0; return d; }
void CObjectRotateCamera::SetCenter(float x, float y, float z) { SetLookat(x, y, z); m_center = Point3(x, y, z); Point3 eye = Point3(vEyePt.x, vEyePt.y, vEyePt.z); Point3 dir = eye - m_center; m_radius = dir.GetLength(); Point3 angle = Dir2Angle(eye - m_center); m_anglex = angle.x; m_anglez = angle.y; }
bool IsScaled(Matrix3& tm) { Matrix3 t(1); t = tm - t; Point3 x = t.GetRow(0), y = t.GetRow(1), z = t.GetRow(2), u = t.GetRow(3); float v = x.LengthSquared() + y.LengthSquared() + z.LengthSquared() + u.LengthSquared(); if(v > gTolerenceEpsilon) return true; else return false; }
void HeightGrid::init() { if (m_heightImage != NULL) { clog << "Generate terrain data: " << m_heightImage->getWidth() << ", " << m_heightImage->getHeight() << ", Format: " << m_heightImage->getFormat() << ", width step: " << m_heightImage->getWidthStep() << endl; MyImage<BGRPixel> img(m_heightImage); // TODO: Compute normals. float scaleZ = m_max - m_min; if (scaleZ < 0) { scaleZ = 1.0f; } if ( (m_ground < 0) || (m_ground > 1) ) { m_ground = 0.5; } clog << "Ground height: " << m_ground << ", scaling Z : " << scaleZ << ", translation for z-direction: " << (-1 * scaleZ * m_ground) << endl; m_callList = glGenLists(1); glNewList(m_callList, GL_COMPILE); for (uint32_t y = 0; y < m_heightImage->getHeight() - 2; y++) { glBegin(GL_TRIANGLE_STRIP); for (uint32_t x = 0; x < m_heightImage->getWidth(); x++) { glColor3f(static_cast<int>(img.getPixel(x, m_heightImage->getHeight() - 1 - y)->r) / 255.0f, static_cast<int>(img.getPixel(x, m_heightImage->getHeight() - 1 - y)->r) / 255.0f, static_cast<int>(img.getPixel(x, m_heightImage->getHeight() - 1 - y)->r) / 255.0f); glVertex3f(static_cast<float>(x), static_cast<float>(y), (static_cast<int>(img.getPixel(x, m_heightImage->getHeight() - 1 - y)->r) / 255.0f - m_ground) * scaleZ); glVertex3f(static_cast<float>(x), static_cast<float>(y + 1), (static_cast<int>(img.getPixel(x, m_heightImage->getHeight() - 1 - (y + 1))->r) / 255.0f - m_ground) * scaleZ); } glEnd(); } glEndList(); // Compute translation. Point3 translate; translate.setX(-1 * m_originPixelXY.getX() * m_scalingPixelXY.getX()); translate.setY(-1 * (m_heightImage->getHeight() - m_originPixelXY.getY()) * m_scalingPixelXY.getY()); Point3 scale(m_scalingPixelXY); // m_scalingPixelXY sets z to 0, thus elevation is disable. Therefore, scale z to 1.0, i.e. keep computed elevation. scale.setZ(1.0); // Set up the actual renderer. m_heightImageRenderer = new HeightGridRenderer(getNodeDescriptor(), m_callList); // Set up transform group. m_heightImageNode = new TransformGroup(); m_heightImageNode->addChild(m_heightImageRenderer); // Translate the height image. m_heightImageNode->setTranslation(Point3(translate.getX(), translate.getY(), 0)); // Scale the height image. m_heightImageNode->setScaling(scale); } }
/// Creates a new coordinate matrix with the coordinates from /// \p conformer. Coordinates::Coordinates(const Conformer *conformer) : m_matrix(conformer->molecule()->size(), 3) { int size = conformer->molecule()->size(); for(int i = 0; i < size; i++){ Point3 position = conformer->position(conformer->molecule()->atom(i)); m_matrix.setValue(i, 0, position.x()); m_matrix.setValue(i, 1, position.y()); m_matrix.setValue(i, 2, position.z()); } }
/// Creates a new coordinate matrix with the coordinates from /// \p atoms. Coordinates::Coordinates(const std::vector<Atom *> &atoms) : m_matrix(atoms.size(), 3) { unsigned int size = atoms.size(); for(unsigned int i = 0; i < size; i++){ Point3 position = atoms[i]->position(); m_matrix.setValue(i, 0, position.x()); m_matrix.setValue(i, 1, position.y()); m_matrix.setValue(i, 2, position.z()); } }
/// Creates a new coordinate matrix with the coordinates from /// \p molecule. Coordinates::Coordinates(const Molecule *molecule) : m_matrix(molecule->size(), 3) { int size = molecule->size(); for(int i = 0; i < size; i++){ Point3 position = molecule->atom(i)->position(); m_matrix.setValue(i, 0, position.x()); m_matrix.setValue(i, 1, position.y()); m_matrix.setValue(i, 2, position.z()); } }
void GCodeExport::writeMoveBFB(int x, int y, int z, double speed, double extrusion_mm3_per_mm) { double extrusion_per_mm = mm3ToE(extrusion_mm3_per_mm); Point gcode_pos = getGcodePos(x,y, current_extruder); //For Bits From Bytes machines, we need to handle this completely differently. As they do not use E values but RPM values. float fspeed = speed * 60; float rpm = extrusion_per_mm * speed * 60; const float mm_per_rpm = 4.0; //All BFB machines have 4mm per RPM extrusion. rpm /= mm_per_rpm; if (rpm > 0) { if (extruder_attr[current_extruder].retraction_e_amount_current) { if (currentSpeed != double(rpm)) { //fprintf(f, "; %f e-per-mm %d mm-width %d mm/s\n", extrusion_per_mm, lineWidth, speed); //fprintf(f, "M108 S%0.1f\r\n", rpm); *output_stream << "M108 S" << std::setprecision(1) << rpm << new_line; currentSpeed = double(rpm); } //Add M101 or M201 to enable the proper extruder. *output_stream << "M" << int((current_extruder + 1) * 100 + 1) << new_line; extruder_attr[current_extruder].retraction_e_amount_current = 0.0; } //Fix the speed by the actual RPM we are asking, because of rounding errors we cannot get all RPM values, but we have a lot more resolution in the feedrate value. // (Trick copied from KISSlicer, thanks Jonathan) fspeed *= (rpm / (roundf(rpm * 100) / 100)); //Increase the extrusion amount to calculate the amount of filament used. Point3 diff = Point3(x,y,z) - getPosition(); current_e_value += extrusion_per_mm * diff.vSizeMM(); } else { //If we are not extruding, check if we still need to disable the extruder. This causes a retraction due to auto-retraction. if (!extruder_attr[current_extruder].retraction_e_amount_current) { *output_stream << "M103" << new_line; extruder_attr[current_extruder].retraction_e_amount_current = 1.0; // 1.0 used as stub; BFB doesn't use the actual retraction amount; it performs retraction on the firmware automatically } } *output_stream << std::setprecision(3) << "G1 X" << INT2MM(gcode_pos.X) << " Y" << INT2MM(gcode_pos.Y) << " Z" << INT2MM(z) << std::setprecision(1) << " F" << fspeed << new_line; currentPosition = Point3(x, y, z); estimateCalculator.plan(TimeEstimateCalculator::Position(INT2MM(currentPosition.x), INT2MM(currentPosition.y), INT2MM(currentPosition.z), eToMm(current_e_value)), speed); }
/** * @param includeMargin Indicate whether algorithm operates on objects with margin */ template<class T> std::unique_ptr<GJKResult<T>> GJKAlgorithm<T>::processGJK(const CollisionConvexObject3D &convexObject1, const CollisionConvexObject3D &convexObject2, bool includeMargin) const { //get point which belongs to the outline of the shape (Minkowski difference) Vector3<T> initialDirection = Vector3<T>(1.0, 0.0, 0.0); Point3<T> initialSupportPointA = convexObject1.getSupportPoint(initialDirection.template cast<float>(), includeMargin).template cast<T>(); Point3<T> initialSupportPointB = convexObject2.getSupportPoint((-initialDirection).template cast<float>(), includeMargin).template cast<T>(); Point3<T> initialPoint = initialSupportPointA - initialSupportPointB; Vector3<T> direction = (-initialPoint).toVector(); Simplex<T> simplex; simplex.addPoint(initialSupportPointA, initialSupportPointB); T minimumToleranceMultiplicator = (T)1.0; for(unsigned int iterationNumber=0; iterationNumber<maxIteration; ++iterationNumber) { Point3<T> supportPointA = convexObject1.getSupportPoint(direction.template cast<float>(), includeMargin).template cast<T>(); Point3<T> supportPointB = convexObject2.getSupportPoint((-direction).template cast<float>(), includeMargin).template cast<T>(); Point3<T> newPoint = supportPointA - supportPointB; const Vector3<T> &vClosestPoint = -direction; //vector from origin to closest point of simplex T closestPointSquareDistance = vClosestPoint.dotProduct(vClosestPoint); T closestPointDotNewPoint = vClosestPoint.dotProduct(newPoint.toVector()); //check termination conditions: new point is not more extreme that existing ones OR new point already exist in simplex T distanceTolerance = std::max(minimumTerminationTolerance*minimumToleranceMultiplicator, relativeTerminationTolerance*closestPointSquareDistance); if((closestPointSquareDistance-closestPointDotNewPoint) <= distanceTolerance || simplex.isPointInSimplex(newPoint)) { if(closestPointDotNewPoint <= 0.0) { //collision detected return std::make_unique<GJKResultCollide<T>>(simplex); }else { return std::make_unique<GJKResultNoCollide<T>>(std::sqrt(closestPointSquareDistance), simplex); } } simplex.addPoint(supportPointA, supportPointB); direction = (-simplex.getClosestPointToOrigin()).toVector(); minimumToleranceMultiplicator += percentageIncreaseOfMinimumTolerance; } #ifdef _DEBUG logMaximumIterationReach(); #endif return std::make_unique<GJKResultInvalid<T>>(); }
/* * compute the angle formed by v0-v1-v2 */ static inline T angle(const Point3<T>& v0, const Point3<T>& v1, const Point3<T>& v2) { T a2 = v0.distanceSqr(v1); T a = sqrt(a2); T b2 = v2.distanceSqr(v1); T b = sqrt(b2); T s = 2. * a * b; if ( fabs(s) < 1E-12 ) fprintf(stderr, "ERROR: zero-length edge encountered\n"); return acos((a2 + b2 - v0.distanceSqr(v2)) / s); }
Point3 Transformation::transformInversely(const Point3 &coordinate, const Position &position) const { Point3 cc = coordinate; // Rotate the coordinate. cc.rotateX(-1 * position.getRotation().getX()); cc.rotateY(-1 * position.getRotation().getY()); cc.rotateZ(-1 * position.getRotation().getZ()); // Translate the coordinate. cc -= position.getPosition(); return cc; }
bool lexic(Point3 P1, Point3 P2) { return P1.getX() < P2.getX() || P1.getX() == P2.getX() && P1.getY() < P2.getY() || P1.getX() == P2.getX() && P1.getY() == P2.getY() && P1.getZ() < P2.getZ(); }
void MatchWindow::updatePoseText () { Point3 worldPos = glcanvas->pose()->getWorldPosition(); double heading = (glcanvas->pose()->getHeading()) * 180.0/M_PI, elevation = glcanvas->pose()->getElevation() * 180.0/M_PI, bank = glcanvas->pose()->getBank() * 180.0/M_PI; wposx->setText (worldPos.x()); wposy->setText (worldPos.y()); wposz->setText (worldPos.z()); wyaw->setText (heading); wpitch->setText (elevation); wroll->setText (bank); }
/* ************************************************************************* */ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, OptionalJacobian<3,3> Dpoint) const { // Only get matrix once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 R = R_.matrix(); if (Dpose) { Dpose->leftCols<3>() = R * skewSymmetric(-p.x(), -p.y(), -p.z()); Dpose->rightCols<3>() = R; } if (Dpoint) { *Dpoint = R; } return Point3(R * p.vector()) + t_; }
Point3 getRefractionVector(Point3 view, Point3 normal, float n1, float n2) { Point3 Vnormal = normal; float cosThetaOne = view.Dot(normal) / view.Length(); float sinThetaTwo = (n1 / n2) * sqrt(1 - pow(cosThetaOne, 2)); float cosThetaTwo = sqrt(1 - pow(sinThetaTwo, 2)); Point3 Vt = (view - (view.Dot(normal)*normal)).GetNormalized(); /******************Total Internal Reflection **************************/ //if (sinThetaTwo > 1 || sinThetaTwo < -1) //Checking total interanl reflection // return(getReflectionVector(-view, normal)); //If happened returning a reflection vector /*************************End Total Internal Reflection***************/ return ((cosThetaTwo * -1 * Vnormal) + (sinThetaTwo * -1 * Vt)); }
inline Angle direction( const Point3 & p1, const Point3 & p2 ) { qreal hypotenuse = distance( p1, p2 ); if ( qFuzzyCompare( hypotenuse, 0 ) ) return Angle(); Angle angle( asin( ( p2.x() - p1.x() ) / hypotenuse ) ); return ( ( p2.y() - p1.y() ) < 0 ) ? Angle::normalize( M_PI - angle.radian() ) : angle; }
/* ************************************************************************* */ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) const { Matrix36 D_local_pose; Matrix3 D_local_point; Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); if (!H1 && !H2) { return local.norm(); } else { Matrix13 D_r_local; const double r = local.norm(D_r_local); if (H1) *H1 = D_r_local * D_local_pose; if (H2) *H2 = D_r_local * D_local_point; return r; } }
// Calculate bounding sphere using average position of the points. Better fit but slower. void CalcCenteredSphere(Mesh& mesh, Point3& center, float& radius) { int nv = mesh.getNumVerts(); Point3 sum(0.0f, 0.0f, 0.0f); for (int i=0; i<nv; ++i) sum += mesh.getVert(i); center = sum / float(nv); float radsq = 0.0f; for (int i=0; i<nv; ++i){ Point3 diff = mesh.getVert(i) - center; float mag = diff.LengthSquared(); radsq = max(radsq, mag); } radius = Sqrt(radsq); }
Point3 plDistributor::IPerpAxis(const Point3& p) const { const float kMinLengthSquared = 1.e-1f; int minAx = p.MinComponent(); Point3 ax(0,0,0); ax[minAx] = 1.f; Point3 perp = p ^ ax; if( perp.LengthSquared() < kMinLengthSquared ) { // hmm, think we might be screwed, but this shouldn't happen. } return perp = perp.FNormalize(); }
/** * @brief * Returns the position, rotation and scale of the scene node at a given time */ void PLSceneNode::GetPosRotScale(Point3 &vPos, Quat &qRot, Point3 &vScale, TimeValue nTime) { if (m_pIGameNode) { // Get the position, rotation and scale - relative to the parent node GMatrix mParentMatrix; IGameNode *pIGameNodeParent = m_pIGameNode->GetNodeParent(); if (pIGameNodeParent) mParentMatrix = pIGameNodeParent->GetWorldTM(nTime); PLTools::GetPosRotScale(m_pIGameNode->GetWorldTM(nTime)*PLTools::Inverse(mParentMatrix), vPos, qRot, vScale, IsRotationFlipped()); // Get the scale (NOT done for special nodes!) if (m_nType != TypeContainer && m_nType != TypeScene && m_nType != TypeCell && m_nType != TypeCamera && m_nType != TypeLight) { // [TODO] Do we still need this hint? // Check for none uniform scale // if (m_vScale.x != m_vScale.y || m_vScale.x != m_vScale.z || m_vScale.y != m_vScale.z) { // We have to use '%e' because else we may get output like '(1 1 1) is no uniform scale' // g_pLog->LogFLine(PLLog::Hint, "Node '%s' has a none uniform scale. (%e %e %e) This 'may' cause problems in special situations...", m_sName.c_str(), m_vScale.x, m_vScale.y, m_vScale.z); // } } else { // Set scale to 1 vScale.Set(1.0f, 1.0f, 1.0f); } } }
void DistanceToObjectsReport::report(const odcore::wrapper::Time &t) { cerr << "Call to DistanceToObjectsReport for t = " << t.getSeconds() << "." << t.getPartialMicroseconds() << ", containing " << getFIFO().getSize() << " containers." << endl; // Get last EgoState. KeyValueDataStore &kvds = getKeyValueDataStore(); Container c = kvds.get(opendlv::data::environment::EgoState::ID()); EgoState es = c.getData<EgoState>(); const uint32_t SIZE = getFIFO().getSize(); for (uint32_t i = 0; i < SIZE; i++) { c = getFIFO().leave(); cerr << "Received: " << c.toString() << endl; if (c.getDataType() == opendlv::data::environment::Obstacle::ID()) { Obstacle o = c.getData<Obstacle>(); const float DISTANCE = (es.getPosition().getDistanceTo(o.getPosition())); cerr << "DistanceToObjectsReport: Distance to object: " << DISTANCE << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << endl; // Continuously check distance. m_correctDistance &= (DISTANCE > m_threshold); vector<Point3> shape = o.getPolygon().getVertices(); Point3 head = shape.front(); shape.push_back(head); const uint32_t NUMVERTICES = shape.size(); for(uint32_t j = 1; j < NUMVERTICES; j++) { Point3 pA = shape.at(j-1); Point3 pB = shape.at(j); // TODO: Check polygonal data as well as perpendicular to all sides. // Create line. Line l(pA, pB); // Compute perpendicular point. Point3 perpendicularPoint = l.getPerpendicularPoint(es.getPosition()); // Compute distance between current position and perpendicular point. const float DISTANCE_PP = (es.getPosition().getDistanceTo(perpendicularPoint)); cerr << "DistanceToObjectsReport: Distance to object's shape: " << DISTANCE_PP << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << ", perpendicular point:" << perpendicularPoint.toString() << endl; // Continuously check distance. m_correctDistance &= (DISTANCE > m_threshold); } } if (c.getDataType() == opendlv::data::environment::OtherVehicleState::ID()) { OtherVehicleState o = c.getData<OtherVehicleState>(); const float DISTANCE = (es.getPosition().getDistanceTo(o.getPosition())); // Compute distance between current position and perpendicular point. cerr << "DistanceToObjectsReport: Distance to other vehicle: " << DISTANCE << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << endl; // Continuously check distance. m_correctDistance &= (DISTANCE > m_threshold); } } }
void getOrthoNormalBasisVector(Point3 i_up, Point3 &o_out_vector /*U*/, Point3& o_vector_right /*v*/) { Point3 randomVectorW; //bool foundRandomVector = false; while (true) { randomVectorW = getRandomVector(); if ( fabs(i_up.Dot(randomVectorW)) < RANDOMCOSINEANGLE) { o_out_vector = i_up.Cross(randomVectorW); o_vector_right = i_up.Cross(o_out_vector).GetNormalized(); o_out_vector.Normalize(); break; } } }
void printBox3D(Box3D box) { Point3 min = box.min(); Point3 max = box.max(); printf("<%f,%f,%f> to <%f,%f,%f>\n", min.x(), min.y(), min.z(), max.x(), max.y(), max.z()); fflush(stdout); }
static inline bool point_is_spike_or_equal(Point1 const& last_point, Point2 const& segment_a, Point3 const& segment_b) { // adapted from boost\geometry\algorithms\detail\point_is_spike_or_equal.hpp to include tolerance checking // segment_a is at the beginning // segment_b is in the middle // last_point is at the end // segment_b is being considered for deletion double normTol = 0.001; // 1 mm double tol = 0.001; // relative to 1 double diff1_x = last_point.x()-segment_b.x(); double diff1_y = last_point.y()-segment_b.y(); double norm1 = sqrt(pow(diff1_x, 2) + pow(diff1_y, 2)); if (norm1 > normTol){ diff1_x = diff1_x/norm1; diff1_y = diff1_y/norm1; }else{ // last point is too close to segment b return true; } double diff2_x = segment_b.x()-segment_a.x(); double diff2_y = segment_b.y()-segment_a.y(); double norm2 = sqrt(pow(diff2_x, 2) + pow(diff2_y, 2)); if (norm2 > normTol){ diff2_x = diff2_x/norm2; diff2_y = diff2_y/norm2; }else{ // segment b is too close to segment a return true; } double crossProduct = diff1_x*diff2_y-diff1_y*diff2_x; if (abs(crossProduct) < tol){ double dotProduct = diff1_x*diff2_x+diff1_y*diff2_y; if (dotProduct <= -1.0 + tol){ // reversal return true; } } return false; }
/* ************************************************************************* */ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, OptionalJacobian<3,3> Dpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); const Point3 q(Rt*(p - t_).vector()); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); (*Dpose) << 0.0, -wz, +wy,-1.0, 0.0, 0.0, +wz, 0.0, -wx, 0.0,-1.0, 0.0, -wy, +wx, 0.0, 0.0, 0.0,-1.0; } if (Dpoint) { *Dpoint = Rt; } return q; }
float movingDiskFixedLineSegmentCollisionTime(Point2 center, float radius, Vector2 velocity, const LineSegment2D& segment, Point2& collisionLocation) { // Ridiculously implemented in terms of existing 3D functionality Point3 X; const float t = CollisionDetection::collisionTimeForMovingSphereFixedTriangle (Sphere(Point3(center, 0.0f), radius), Vector3(velocity, 0.0f), Triangle(Point3(segment.point(0), +1.0f), Point3(segment.point(0), -1.0f), Point3(segment.point(1), 0.0f)), X); if (t < finf()) { collisionLocation = X.xy(); } return t; }