Пример #1
0
bool Line::intersect(
	float pickAngle,		// The angle which makes the cone
	const Vec3f& v0,		// One endpoint of the line segment
	const Vec3f& v1,		// The other endpoint of the line segment
	Vec3f& intersection) const	// The intersection point
{
	Vec3f	ptOnLine;
	Line	inputLine(v0, v1);
	float	distance;
	bool    validIntersection = false;

	if(getClosestPoints(inputLine, ptOnLine, intersection)) 
	{
		// check to make sure the intersection is within the line segment
		if((intersection - v0).dot(v1 - v0) < 0)
		{
			intersection = v0;
		}
		else if((intersection - v1).dot(v0 - v1) < 0)
		{
			intersection = v1;
		}

		distance = (ptOnLine - intersection).getLength();
		
		if (pickAngle < 0.0)		return (distance < -pickAngle);

		validIntersection = ((distance / (ptOnLine - getPosition()).getLength())
						  < pickAngle);
	}

	return validIntersection;
}
Пример #2
0
/**
 * Fits a plane to a 3D point cloud of data. The returned matrix is in the following format:
 * First column: plane normal
 * Next 4 columns: 4 corners of the plane that contains all the data
 */
Mat TableObjectDetector::fitPlane(const Mat depthWorld) {
    Mat P = getClosestPoints(depthWorld, 1.0);
    Mat Pmean(1, 3, CV_64F);
    Pmean.at<double>(0) = mean(P.col(0))[0];
    Pmean.at<double>(1) = mean(P.col(1))[0];
    Pmean.at<double>(2) = mean(P.col(2))[0];
        
    for (int i=0; i<P.rows; i++) {
        P.row(i) = P.row(i)-Pmean;
    }
    Mat PT; transpose(P, PT);
    Mat A = PT*P;
    // Find normal of LSQ plane
    SVD svd(A, SVD::FULL_UV);
    Mat norm; transpose(svd.vt.row(2), norm);
    
    double nx = norm.at<double>(0);
    double ny = norm.at<double>(1);
    double nz = norm.at<double>(2);   
    double rho = (Pmean.at<double>(0)*nx +
                  Pmean.at<double>(1)*ny +
                  Pmean.at<double>(2)*nz);

    // Put the normal in the plane matrix
    Mat plane(3, 5, CV_64F);
    norm.col(0).copyTo(plane.col(0));

    // Generate corners of the plane
    double xmin, ymin, xmax, ymax;
    xmin = -0.5;
    xmax = 0.5;
    ymin = -0.5;
    ymax = 0.1;
    
    plane.at<double>(0, 1) = xmin;
    plane.at<double>(0, 2) = xmax;
    plane.at<double>(0, 3) = xmax;
    plane.at<double>(0, 4) = xmin;
    plane.at<double>(1, 1) = ymin;
    plane.at<double>(1, 2) = ymin;
    plane.at<double>(1, 3) = ymax;
    plane.at<double>(1, 4) = ymax;
    double z1 = (rho-nx*xmin-ny*ymin)/nz;
    double z2 = (rho-nx*xmax-ny*ymin)/nz;
    double z3 = (rho-nx*xmax-ny*ymax)/nz;
    double z4 = (rho-nx*xmin-ny*ymax)/nz;
    plane.at<double>(2, 1) = z1;
    plane.at<double>(2, 2) = z2;
    plane.at<double>(2, 3) = z3;
    plane.at<double>(2, 4) = z4;

    return plane;
}
Пример #3
0
void GjkCollisionState::getCollisionInfo(const MatrixF& mat, Collision* info)
{
   AssertFatal(false, "GjkCollisionState::getCollisionInfo() - There remain scaling problems here.");
   // This assumes that the shapes do not intersect
   Point3F pa,pb;
   if (bits) {
      getClosestPoints(pa,pb);
      mat.mulP(pa,&info->point);
      b->getTransform().mulP(pb,&pa);
      info->normal = info->point - pa;
   }
   else {
      mat.mulP(p[last],&info->point);
      info->normal = v;
   }
   info->normal.normalize();
   info->object = b->getObject();
}
Пример #4
0
/**
 * Fit a plane using RANSAC. c/o Varsha Shankar
 */
Mat TableObjectDetector::fitPlaneRANSAC(const Mat depthWorld) {
    Mat P = getClosestPoints(depthWorld, 1.0);
    Mat plane = Mat::zeros(3, 5, CV_64F);
    int N = P.rows;
    int n_iter = 30;
    float thresh = 0.001;
    int n_inliers = 0;
    Mat n_est;

    int current_n_inliers = 0;
    Mat dist;
    for (int i=0; i<n_iter; i++) {
        // Randomly pick 3 points
        Mat randPoints(3, 3, CV_64F);
        for (int j=0; j<3; j++) {
            int r_idx = rand() % N;
            P.row(r_idx).copyTo(randPoints.row(j));
        }

        // Calculate the plane that these points lie on
        Mat normal = (randPoints.row(0)-randPoints.row(1)).cross(randPoints.row(0)-randPoints.row(2));
        normal = normal/cv::norm(normal);

        // Calculate the distance of each point to this plane
        Mat v(P.rows, P.cols, CV_64F);
        for (int j=0; j<N; j++) {
            v.row(j) = P.row(j)-randPoints.row(0);
        }
        transpose(normal, normal);
        dist = cv::abs(v*normal);

        // Calculate inliers
        current_n_inliers = 0;
        for (int j=0; j<N; j++) {
            if (dist.at<double>(j) < thresh) {
                current_n_inliers++;
            }  
        }

        // Update if this is a good fit
        if (current_n_inliers > n_inliers) {
            n_est = normal;
            n_inliers = current_n_inliers;
        }

        cout << "RANSAC step " << i << "/" << n_iter << endl;

    }

    n_est.copyTo(plane.col(0));
    int n_inlier_pts = 0;
    while (n_inlier_pts < 4) {
        int r_idx = rand() % N;
        if (dist.at<double>(r_idx) < thresh) {
            Mat rand_pt;
            P.row(r_idx).copyTo(rand_pt);
            transpose(rand_pt, rand_pt);
            rand_pt.copyTo(plane.col(n_inlier_pts+1));
            n_inlier_pts++;
        }


    }



    return plane;

}
// applyAction Called by the simulation every frame.
void PlanetGravityAction::applyAction( const hkStepInfo& stepInfo )
{
	hkpRigidBody* rb = getRigidBody();

	// Sets forceDir to be the difference of m_center and rb->getCenterOfMassInWorld().
	hkVector4 forceDir;

	hkpCollisionInput input = *getWorld()->getCollisionInput();
	hkpClosestCdPointCollector collector;
	hkpShapeType a = rb->getCollidable()->getShape()->getType();
	input.m_tolerance = 1.0f;

	do {
		// Use m_hullCollidable for gravity calculations if available.
		// GetClosestPoints() should provide a gravity vector
		if( m_hullCollidable == HK_NULL)
		{
			hkpShapeType b = m_planetBody->getCollidable()->getShape()->getType();
			hkpCollisionDispatcher::GetClosestPointsFunc getClosestPoints = getWorld()->getCollisionDispatcher()->getGetClosestPointsFunc( a, b );
			getClosestPoints( *rb->getCollidable(), *m_planetBody->getCollidable(), input, collector );
		}
		else
		{
			hkpShapeType b = m_hullCollidable->getShape()->getType();
			hkpCollisionDispatcher::GetClosestPointsFunc getClosestPoints = getWorld()->getCollisionDispatcher()->getGetClosestPointsFunc( a, b );
			getClosestPoints( *rb->getCollidable(), *m_hullCollidable, input, collector );	
		}

		// Grow the tolerance until we get a hit.
		input.m_tolerance = input.m_tolerance * 2.0f;
	} while( !collector.hasHit() );

	// Flip the normal of the getClosestPoints() result
	forceDir.setMul4( -1.0f, collector.getHitContact().getNormal() );
	forceDir.normalize3();
	forceDir.zeroElement( 3 );

	// Set force to be direction multiplied by magnitude multiplied by mass.
	hkVector4 force;
	force.setMul4( rb->getMass() * m_gravityForce, forceDir );

	// Apply the gravity force.
	if( rb == PlanetGravityDemo::m_characterRigidBody->getRigidBody() )
	{
		hkVector4 newUp(forceDir);
		newUp.setNeg4(forceDir);
		newUp.normalize3();

		// Only change gravity if the change isn't negligible
		if( PlanetGravityDemo::m_worldUp.dot3(newUp) < 1e-6f)
		{
			// don't reorient character when it has an invalid rotational axis
			//  (this may happen the first few frames)
			if( PlanetGravityDemo::m_characterRigidBody->getRigidBody()->getRotation().hasValidAxis())
			{
				hkRotation rbRotation; rbRotation.set(PlanetGravityDemo::m_characterRigidBody->getRigidBody()->getRotation());
				hkVector4& oldForward = rbRotation.getColumn(0);
				hkVector4 newRot; newRot.setCross(oldForward, newUp);
 				PlanetGravityDemo::m_characterForward.setCross(newUp, newRot);
 				PlanetGravityDemo::m_characterForward.normalize3();
			}
		}

		PlanetGravityDemo::m_worldUp = newUp;
	}
	else
	{
		rb->applyForce( stepInfo.m_deltaTime, force );
	}
}