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