virtual bool Initialize() {
viewport.n = 0.1;
viewport.f = 100;
viewport.setLensAngle(DtoR(30.0));
camera.dist = 6;
if(!geometry.empty()) {
AABB3D bb;
bb.minimize();
for(size_t i=0;i<geometry.size();i++) {
AABB3D bi = geometry[i]->GetAABB();
bb.expand(bi.bmin);
bb.expand(bi.bmax);
}
camera.tgt = (bb.bmin + bb.bmax)*0.5;
Real size = (bb.bmax-bb.bmin).maxElement();
camera.dist = 3.0*size;
if(size < 0.1) viewport.n = size;
if(size*3 > 100) viewport.f = size*3;
}
glEnable(GL_CULL_FACE);
glEnable(GL_DEPTH_TEST);
glClearColor(0,0,0,0);
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
return GLUTNavigationProgram::Initialize();
}
Real VolumeGrid::Average(const AABB3D& range) const
{
IntTriple imin,imax;
GetIndexRange(range,imin,imax);
//check range
if(imax.a < 0 || imax.b < 0 || imax.c < 0) {
return 0;
}
if(imin.a >= value.m || imin.b >= value.n || imax.c >= value.p) {
return 0;
}
//limit range
if(imin.a < 0) imin.a=0;
if(imin.b < 0) imin.b=0;
if(imin.c < 0) imin.c=0;
if(imax.a >= value.m) imax.a=value.m-1;
if(imax.b >= value.n) imax.b=value.n-1;
if(imax.c >= value.p) imax.c=value.p-1;
bool ignoreX=(range.bmin.x==range.bmax.x),ignoreY=(range.bmin.y==range.bmax.y),ignoreZ=(range.bmin.z==range.bmax.z);
Vector3 cellcorner;
Vector3 cellsize;
cellsize.x = (bb.bmax.x-bb.bmin.x)/Real(value.m);
cellsize.y = (bb.bmax.y-bb.bmin.y)/Real(value.n);
cellsize.z = (bb.bmax.z-bb.bmin.z)/Real(value.p);
Real sumValue=0;
Real sumVolume=0;
cellcorner.x = bb.bmin.x + imin.a*cellsize.x;
for(int i=imin.a;i<=imax.a;i++,cellcorner.x += cellsize.x) {
cellcorner.y = bb.bmin.y + imin.b*cellsize.y;
for(int j=imin.b;j<=imax.b;j++,cellcorner.y += cellsize.y) {
cellcorner.z = bb.bmin.z + imin.c*cellsize.z;
for(int k=imin.c;k<=imax.c;k++,cellcorner.z += cellsize.z) {
AABB3D intersect;
intersect.bmin=cellcorner;
intersect.bmax=cellcorner+cellsize;
intersect.setIntersection(range);
Vector3 isectsize=intersect.bmax-intersect.bmin;
//due to rounding errors, may have negative sizes
if(isectsize.x < 0 || isectsize.y < 0 || isectsize.z < 0) continue;
Real volume=1;
if(!ignoreX) volume*=isectsize.x;
if(!ignoreY) volume*=isectsize.y;
if(!ignoreZ) volume*=isectsize.z;
sumValue += volume*value(i,j,k);
sumVolume += volume;
}
}
}
Vector3 rangesize=range.bmax-range.bmin;
Real rangeVolume = 1;
if(!ignoreX) rangeVolume*=rangesize.x;
if(!ignoreY) rangeVolume*=rangesize.y;
if(!ignoreZ) rangeVolume*=rangesize.z;
Assert(sumVolume < rangeVolume + Epsilon);
return sumValue / rangeVolume;
}
void Polygon3D::getAABB(AABB3D& bb) const
{
if(vertices.size() == 0) {
bb.minimize();
return;
}
bb.setPoint(vertices[0]);
for(size_t i=1; i<vertices.size(); i++)
bb.expand(vertices[i]);
}
void NearbyPoints(const CollisionPointCloud& pc,const GeometricPrimitive3D& g,Real tol,std::vector<int>& pointIds,size_t maxContacts)
{
Box3D bb;
GetBB(pc,bb);
//quick reject test
if(g.Distance(bb) > tol) return;
GeometricPrimitive3D glocal = g;
RigidTransform Tinv;
Tinv.setInverse(pc.currentTransform);
glocal.Transform(Tinv);
AABB3D gbb = glocal.GetAABB();
gbb.setIntersection(pc.bblocal);
//octree overlap method
vector<Vector3> points;
vector<int> ids;
pc.octree->BoxQuery(gbb.bmin-Vector3(tol),gbb.bmax+Vector3(tol),points,ids);
for(size_t i=0;i<points.size();i++)
if(glocal.Distance(points[i]) <= tol) {
pointIds.push_back(ids[i]);
if(pointIds.size()>=maxContacts) return;
}
return;
/*
//grid enumeration method
GridSubdivision::Index imin,imax;
pc.grid.PointToIndex(Vector(3,gbb.bmin),imin);
pc.grid.PointToIndex(Vector(3,gbb.bmax),imax);
int numCells = (imax[0]-imin[0]+1)*(imax[1]-imin[1]+1)*(imax[2]-imin[2]+1);
if(numCells > (int)pc.points.size()) {
printf("Testing all points\n");
//test all points, linearly
for(size_t i=0;i<pc.points.size();i++)
if(glocal.Distance(pc.points[i]) <= tol) {
pointIds.push_back(int(i));
if(pointIds.size()>=maxContacts) return;
}
}
else {
printf("Testing points in BoxQuery\n");
gNearbyTestThreshold = tol;
gNearbyTestResults.resize(0);
gNearbyTestObject = &glocal;
gNearbyTestBranch = maxContacts;
pc.grid.BoxQuery(Vector(3,gbb.bmin),Vector(3,gbb.bmax),nearbyTest);
pointIds.resize(gNearbyTestResults.size());
for(size_t i=0;i<points.size();i++)
pointIds[i] = gNearbyTestResults[i] - &pc.points[0];
}
*/
}
AABB3D FormTriangle::getAABB() {
AABB3D aabb;
aabb._lowerBound = aabb._upperBound = _points[0];
aabb.expand(_points[1]);
aabb.expand(_points[2]);
aabb.calculateHalfDims();
aabb.calculateCenter();
return aabb;
}
//------------------------------------------------------------------------------------------------------
//function that checks if sphere collides with a AABB object
//------------------------------------------------------------------------------------------------------
bool Sphere3D::IsColliding(const AABB3D& secondBox) const
{
//make use of AABB's box-sphere collision function
return (secondBox.IsColliding(*this));
}
bool WithinDistance(const CollisionPointCloud& pc,const GeometricPrimitive3D& g,Real tol)
{
Box3D bb;
GetBB(pc,bb);
//quick reject test
if(g.Distance(bb) > tol) return false;
GeometricPrimitive3D glocal = g;
RigidTransform Tinv;
Tinv.setInverse(pc.currentTransform);
glocal.Transform(Tinv);
AABB3D gbb = glocal.GetAABB();
gbb.setIntersection(pc.bblocal);
//octree overlap method
vector<Vector3> points;
vector<int> ids;
pc.octree->BoxQuery(gbb.bmin-Vector3(tol),gbb.bmax+Vector3(tol),points,ids);
for(size_t i=0;i<points.size();i++)
if(glocal.Distance(points[i]) <= tol) return true;
return false;
/*
//grid enumeration method
GridSubdivision::Index imin,imax;
pc.grid.PointToIndex(Vector(3,gbb.bmin),imin);
pc.grid.PointToIndex(Vector(3,gbb.bmax),imax);
int numCells = (imax[0]-imin[0]+1)*(imax[1]-imin[1]+1)*(imax[2]-imin[2]+1);
if(numCells > (int)pc.points.size()) {
//test all points, linearly
for(size_t i=0;i<pc.points.size();i++)
if(glocal.Distance(pc.points[i]) <= tol) return true;
return false;
}
else {
gWithinDistanceTestThreshold = tol;
gWithinDistanceTestObject = &glocal;
bool collisionFree = pc.grid.IndexQuery(imin,imax,withinDistanceTest);
return !collisionFree;
}
*/
}
AABB3D AABB3D::getTransformedAABB(const Matrix4x4f &mat) const {
AABB3D transformedAABB;
Vec3f newCenter(mat * _center);
transformedAABB._lowerBound = newCenter;
transformedAABB._upperBound = newCenter;
// Loop through all corners, transform, and compare
for (int x = -1; x <= 1; x += 2)
for (int y = -1; y <= 1; y += 2)
for (int z = -1; z <= 1; z += 2) {
Vec3f corner(x * _halfDims.x + _center.x, y * _halfDims.y + _center.y, z * _halfDims.z + _center.z);
// Transform the corner
corner = mat * corner;
// Compare bounds
if (corner.x > transformedAABB._upperBound.x)
transformedAABB._upperBound.x = corner.x;
if (corner.y > transformedAABB._upperBound.y)
transformedAABB._upperBound.y = corner.y;
if (corner.z > transformedAABB._upperBound.z)
transformedAABB._upperBound.z = corner.z;
if (corner.x < transformedAABB._lowerBound.x)
transformedAABB._lowerBound.x = corner.x;
if (corner.y < transformedAABB._lowerBound.y)
transformedAABB._lowerBound.y = corner.y;
if (corner.z < transformedAABB._lowerBound.z)
transformedAABB._lowerBound.z = corner.z;
}
// Move from local into world space
transformedAABB.calculateHalfDims();
transformedAABB.calculateCenter();
return transformedAABB;
}
void Circle3D::getAABB(AABB3D& aabb) const
{
aabb.setPoint(center);
Real x,y,z;
x = pythag_leg(axis.x,One)*radius;
y = pythag_leg(axis.y,One)*radius;
z = pythag_leg(axis.z,One)*radius;
aabb.bmin.x -= x;
aabb.bmin.y -= y;
aabb.bmin.z -= z;
aabb.bmax.x += x;
aabb.bmax.y += y;
aabb.bmax.z += z;
}
bool StaticPositionModel::loadFromOBJ(const std::string &fileName, AABB3D &aabb, bool useBuffers, bool clearArrays) {
std::ifstream fromFile(fileName);
if (!fromFile.is_open()) {
#ifdef D3D_DEBUG
std::cerr << "Could not load model file " << fileName << std::endl;
#endif
return false;
}
std::string rootName(getRootName(fileName));
std::vector<Vec3f> filePositions;
// Hash map for linking indices to vertex array index for attributes
std::unordered_map<staticPositionMeshIndexType, size_t> indexToVertex;
// Initial extremes
aabb._lowerBound = Vec3f(999999.0f, 999999.0f, 999999.0f);
aabb._upperBound = Vec3f(-999999.0f, -999999.0f, -999999.0f);
std::unordered_map<std::string, size_t> matReferences;
while (!fromFile.eof()) {
// Read line header
std::string line;
getline(fromFile, line);
std::stringstream ss(line);
std::string header;
ss >> header;
if (header == "v") {
// Add vertex
float x, y, z;
ss >> x >> y >> z;
filePositions.push_back(Vec3f(x, y, z));
aabb.expand(Vec3f(x, y, z));
}
else if (header == "f") {
void Segment3D::getAABB(AABB3D& bb) const
{
bb.setPoint(a);
bb.expand(b);
}
void GLUINavigationProgram::CenterCameraOn(const AABB3D& aabb)
{
aabb.getMidpoint(camera.tgt);
}
void ConvexPolyhedron3D::getAABB(AABB3D& b) const
{
b.setPoint(vertices[0]);
for(int i=1; i<numVertices; i++)
b.expand(vertices[i]);
}
void Sphere3D::getAABB(AABB3D& bb) const
{
bb.setPoint(center);
bb.bmin.x-=radius; bb.bmin.y-=radius; bb.bmin.z-=radius;
bb.bmax.x+=radius; bb.bmax.y+=radius; bb.bmax.z+=radius;
}
bool Triangle3D::intersects(const AABB3D& bb) const
{
//trival accept: contains any point
if(bb.contains(a)||bb.contains(b)||bb.contains(c)) return true;
//trivial reject: bboxes don't intersect
AABB3D tribb;
getAABB(tribb);
if(!bb.intersects(tribb)) return false;
//check for other splitting planes
Plane3D p;
getPlane(p);
if(!p.intersects(bb)) return false;
//check planes orthogonal to edge of tri and edge of bb
ClosedInterval bbInt,triInt;
Vector3 edge;
p.offset = Zero;
//x dir
edge.set(1,0,0);
p.normal.setCross(b-a,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
p.normal.setCross(c-b,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
p.normal.setCross(a-c,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
//y dir
edge.set(0,1,0);
p.normal.setCross(b-a,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
p.normal.setCross(c-b,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
p.normal.setCross(a-c,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
//z dir
edge.set(0,0,1);
p.normal.setCross(b-a,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
p.normal.setCross(c-b,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
p.normal.setCross(a-c,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
return true;
}
void Triangle3D::getAABB(AABB3D& bb) const
{
bb.setPoint(a);
bb.expand(b);
bb.expand(c);
}
bool Sphere3D::intersects(const AABB3D& bb) const
{
Vector3 temp;
return bb.distanceSquared(center,temp) < Sqr(radius);
}
