Geometry::HitResult<double> Section::
intersect(const Geometry::Ray<double,3>& ray, bool cullBackFace) const
{
//find the plane intersection
Geometry::HitResult<double> hit = intersectPlane(ray, cullBackFace);
if (!hit.isValid())
return Geometry::HitResult<double>();
Geometry::Vector<double,3> hitPoint = Geometry::Vector<double,3>(ray(hit.getParameter()));
Geometry::Vector<double,3> startUp = start;
startUp.normalize();
Geometry::Vector<double,3> startToEnd = end - start;
Geometry::Vector<double,3> startTan = startToEnd - (startToEnd*startUp)*startUp;
Geometry::Vector<double,3> startToPoint = hitPoint - start;
if (startTan*startToPoint < 0.0)
return Geometry::HitResult<double>();
Geometry::Vector<double,3> endUp = end;
endUp.normalize();
Geometry::Vector<double,3> endToStart = -startToEnd;
Geometry::Vector<double,3> endTan = endToStart - (endToStart*endUp)*endUp;
Geometry::Vector<double,3> endToPoint = hitPoint - end;
if (endTan*endToPoint < 0.0)
return Geometry::HitResult<double>();
return hit;
}
bool intersectDisk(glm::vec3 const& normal, glm::vec3 const& center,
float const& radius, glm::vec3 const& origin,
glm::vec3 const& direction, float& x)const
{
if (intersectPlane(normal, center, origin, direction, x)) {
glm::vec3 p = origin + direction * x;
glm::vec3 v = p - center;
float d2 = glm::dot(v, v);
return (d2 <= (radius*radius));
}
return false;
}
void Mesh::getIntersection(Ray ray, Intersection &inter, Matrix4x4 trans){
// vector<Face> (face = typedef vector<int> m_faces
// vector<Point3D> m_verts
double t = -1;
double hitTest;
Vector3D normal;
Point3D point;
Intersection boundTest;
boundingSphere->getIntersection(ray, boundTest, trans);
if(!boundTest.isValid()){
return;
}
for(std::vector<Face>::const_iterator F = m_faces.begin();
F != m_faces.end(); ++F){
std::vector<int> points = (*F);
std::vector<Point3D> vertices = std::vector<Point3D>();
for(int i = 0; i < 3; i++){
vertices.push_back(m_verts.at(points.at(i)));
}
Vector3D tnorm = (vertices.at(1) - vertices.at(0)).
cross(vertices.at(2) - vertices.at(0));
double area = tnorm.length();
tnorm.normalize(); // not sure if necessary
hitTest = intersectPlane(ray, vertices.at(0), tnorm);
Point3D hit = ray.point + hitTest*ray.vector;
bool inTri = true;
double areaSum = 0.0;
for(int i = 0; i < 3; i++){
Vector3D cross = (vertices.at((i+1)%3) - hit).cross(vertices.at(i%3) - hit);
areaSum += cross.length();
}
if(std::abs(areaSum - area) > 0.0001) inTri = false;
if(inTri && hitTest > 0 && (t < 0 || t > hitTest)){
t = hitTest;
point = hit;
normal = tnorm;
}
}
if(t < 0)
return;
//Intersection *ans = (Intersection*)malloc(sizeof(Intersection));
inter = Intersection(point, normal, NULL);
}
Vector<double> AncillaryMethods::backprojectGP(const Vector<double> &pos2D, const Camera &cam, const Vector<double> &gp)
{
Vector<double> gpN(3);
gpN(0) = (gp(0));
gpN(1) = (gp(1));
gpN(2) = (gp(2));
Vector<double> ray1;
Vector<double> ray2;
getRay(pos2D, ray1, ray2, cam);
Vector<double> point3D;
intersectPlane(gpN, gp(3), ray1, ray2, point3D);
point3D *= Globals::WORLD_SCALE;
return point3D;
}
double Camera::bbToDetection(const Vector<double>& bbox, Vector<double>& pos3D, double ConvertScale, double& dist)
{
// pos3D.clearContent();
pos3D.setSize(3);
double x = bbox(0);
double y = bbox(1);
double w = bbox(2);
double h = bbox(3);
// bottom_left and bottom_right are the point of the BBOX
Vector<double> bottom_left(3, 1.0);
bottom_left(0) = x + w/2.0;
bottom_left(1) = y + h;
Vector<double> bottom_right(3, 1.0);
bottom_right(0) = x + w;
bottom_right(1) = y + h;
Vector<double> ray_bot_left_1;
Vector<double> ray_bot_left_2;
Vector<double> ray_bot_right_1;
Vector<double> ray_bot_right_2;
// Backproject through base point
getRay(bottom_left, ray_bot_left_1, ray_bot_left_2);
getRay(bottom_right, ray_bot_right_1, ray_bot_right_2);
Vector<double> gpPointLeft;
Vector<double> gpPointRight;
// Intersect with ground plane
intersectPlane(GPN_, GPD_, ray_bot_left_1, ray_bot_left_2, gpPointLeft);
intersectPlane(GPN_, GPD_, ray_bot_right_1, ray_bot_right_2, gpPointRight);
// Find top point
Vector<double> ray_top_1;
Vector<double> ray_top_2;
Vector<double> aux(3, 1.0);
aux(0) = x;
aux(1) = y;
getRay(aux, ray_top_1, ray_top_2);
// Vertical plane through base points + normal
Vector<double> point3;
point3 = gpPointLeft;
point3 -= (GPN_);
Vector<double> vpn(3,0.0);
Vector<double> diffGpo1Point3;
Vector<double> diffGpo2Point3;
diffGpo1Point3 = gpPointLeft;
diffGpo1Point3 -=(point3);
diffGpo2Point3 = gpPointRight;
diffGpo2Point3 -= point3;
vpn = cross(diffGpo1Point3,diffGpo2Point3);
double vpd = (-1.0)*DotProduct(vpn, point3);
Vector<double> gpPointTop;
intersectPlane(vpn, vpd, ray_top_1, ray_top_2, gpPointTop);
// Results
gpPointTop -= gpPointLeft;
// Compute Size
double dSize = gpPointTop.norm();
// Compute Distance
aux = t_;
aux -= gpPointLeft;
dist = aux.norm();
dist = dist * ConvertScale;
if(gpPointLeft(2) < t_(2)) dist = dist * (-1.0);
// Compute 3D Position of BBOx
double posX = gpPointLeft(0) * ConvertScale;
double posY = gpPointLeft(1) * ConvertScale;
double posZ = gpPointLeft(2) * ConvertScale;
pos3D(0) = (posX);
pos3D(1) = (posY);
pos3D(2) = (posZ);
return dSize * ConvertScale;
}
void RockLiningGlobal::action(){
const double PI = 3.14159;
if (openingCreated == true && installed == false){
double angleInterval = 2.0*PI/static_cast<double>(totalNodes);
for (int n=0; n<totalNodes;n++){
double currentAngle = 0.0 + n*angleInterval; /* from 0 degrees east */
double unitX = cos(currentAngle);
double unitY = sin(currentAngle);
Vector3r searchDir(unitX,0,unitY);
vector<double> distanceFrOpening; vector<int> IDs;
double outerRadius = openingRad + 1.0;
FOREACH(const shared_ptr<Body>& b, *scene->bodies){
if (!b) continue;
if (b->isClump() == true) continue;
PotentialBlock* pb=static_cast<PotentialBlock*>(b->shape.get());
if(!pb) continue;
if(pb->isBoundary == true || pb->erase== true || pb->isLining==true){continue;}
State* state1 = b->state.get();
Vector3r intersectionPt(0,0,0);
if ( installLining(pb, state1, startingPoint, searchDir, outerRadius, intersectionPt )){
IDs.push_back(b->id);
distanceFrOpening.push_back((intersectionPt-startingPoint).norm());
//std::cout<<"currentAngle: "<<currentAngle<<", b->id: "<<b->id<<", dist: "<<(intersectionPt-startingPoint).norm()<<endl;
}
}
/* find closest block */
int totalBlocks = IDs.size();
double closestDistance = 100000.0;
int closestID=0;
for (int i=0; i<totalBlocks; i++){
if ( distanceFrOpening[i] < closestDistance){
closestID = IDs[i];
closestDistance = distanceFrOpening[i];
}
}
stickIDs.push_back(closestID);
IDs.clear();distanceFrOpening.clear();
//std::cout<<"closestID: "<<closestID<<endl;
/* find intersection with edges of polygon */
Vector3r jointIntersection (0,0,0);
State* state1 = Body::byId(closestID,scene)->state.get();
Shape* shape1 = Body::byId(closestID,scene)->shape.get();
PotentialBlock *pb=static_cast<PotentialBlock*>(shape1);
int totalPlanes = pb->a.size();
int intersectNo = 0;
Vector3r nodeLocalPos(0,0,0); Vector3r nodeGlobalPos(0,0,0);
//std::cout<<"totalPlanes: "<<totalPlanes<<endl;
double closestPlaneDist = 1000000;
for (int i=0; i<totalPlanes; i++){
Vector3r plane = state1->ori*Vector3r(pb->a[i], pb->b[i], pb->c[i]); double planeD = plane.dot(state1->pos) + pb->d[i] +pb->r;
if ( intersectPlane(pb, state1,startingPoint,searchDir, outerRadius, jointIntersection, plane, planeD)){
double distance = jointIntersection.norm();
if (distance < closestPlaneDist){
closestPlaneDist = distance;
nodeLocalPos = state1->ori.conjugate()*(jointIntersection-state1->pos);
nodeGlobalPos=jointIntersection;
}
}
}
if(nodeGlobalPos.norm() > 1.03*openingRad){ nodeGlobalPos=1.03*openingRad*searchDir;}
//if(nodeGlobalPos.norm() < 0.98*openingRad){ continue;}
//initOverlap = interfaceTension/interfaceStiffness;
nodeGlobalPos = nodeGlobalPos + searchDir*initOverlap;
localCoordinates.push_back(nodeLocalPos);
refPos.push_back(nodeGlobalPos);
int nodeID = insertNode(nodeGlobalPos, lumpedMass, contactLength);
blockIDs.push_back(nodeID); //(nodeID); //(closestID);
refOri.push_back(Quaternionr::Identity()); //(state1->ori);
installed = true;
axialForces.push_back(0.0);
shearForces.push_back(0.0);
moment.push_back(0.0);
sigmaMax.push_back(0.0);
sigmaMin.push_back(0.0);
displacement.push_back(0.0);
radialDisplacement.push_back(0.0);
}
totalNodes=blockIDs.size();
/* Assembling global stiffness matrix */
for (int n=0; n<totalNodes;n++){
int nextID = n+1;
if(nextID==totalNodes){nextID=0;}
double Length = (refPos[nextID]-refPos[n]).norm();
lengthNode.push_back(Length);
Vector3r localDir = refPos[nextID]-refPos[n];
localDir.normalize(); refDir.push_back(localDir);
double angle = acos(localDir.dot(Vector3r(1,0,0)));
Vector3r signAngle = Vector3r(1,0.0,0).cross(localDir);
if (signAngle.dot(Vector3r(0,-1.0,0)) < 0.0){angle =2.0*PI - angle;}
refAngle.push_back(angle);
std::cout<<"angle "<<n<<" : "<<angle/PI*180.0<<endl;
}
}
void gpuCacheIsectUtil::getClosestPointToRayOnLine(const MPoint& vertex1, const MPoint& vertex2, const MPoint& raySource, const MVector& rayDirection, MPoint& closestPoint, double& percent){
MPoint clsPoint;
MVector edgeDir = vertex2 - vertex1;
double len = edgeDir.length();
if(len < 0.0000001 )
{
percent = 0.0;
closestPoint = vertex1;
return;
}
edgeDir.normalize();
//if line is parallel to ray
double dotPrd = fabs(edgeDir * rayDirection);
if(dotPrd > 0.9999){
percent = 0.0;
closestPoint = vertex1;
return;
}
// Vector connecting two closest points.
//
MVector crossProd = edgeDir ^ rayDirection;
// Normal to the plane defined by that vector and the 'otherLine'.
//
MVector planeNormal = rayDirection ^ crossProd;
//intersectionPlane is raySource,planeNormal
double t;
if(intersectPlane(raySource, planeNormal, vertex1,edgeDir,t)){
clsPoint = vertex1 + t * edgeDir;
// Find percent, where
// vertex1 + percent * (edgeDir) == closestPoint
//
percent = edgeDir * (clsPoint - vertex1) / len;
// The closest point may not be on the segment. Find the closest
// point on the segment using t.
//
if (percent < 0)
{
closestPoint = vertex1;
percent = 0.0;
}
else if (percent > 1.0)
{
closestPoint = vertex2;
percent = 1.0;
}
else
{
closestPoint = clsPoint;
}
} else
{
closestPoint = vertex1;
percent = 0.0;
}
}
