void LocalMultiBlockInfo3D::computePeriodicOverlaps (
SparseBlockStructure3D const& sparseBlock, plint blockId )
{
Box3D intersection; // Temporary variable.
std::vector<plint> neighbors; // Temporary variable.
SmartBulk3D bulk(sparseBlock, envelopeWidth, blockId);
for (plint dx=-1; dx<=+1; dx+=1) {
for (plint dy=-1; dy<=+1; dy+=1) {
for (plint dz=-1; dz<=+1; dz+=1) {
if (dx!=0 || dy!=0 || dz!=0) {
// The new block is shifted by the length of the full multi block in each space
// direction. Consequently, overlaps between the original multi block and the
// shifted new block are identified as periodic overlaps.
plint shiftX = dx*sparseBlock.getBoundingBox().getNx();
plint shiftY = dy*sparseBlock.getBoundingBox().getNy();
plint shiftZ = dz*sparseBlock.getBoundingBox().getNz();
Box3D shiftedBulk(bulk.getBulk().shift(shiftX,shiftY,shiftZ));
Box3D shiftedEnvelope(bulk.computeEnvelope().shift(shiftX,shiftY,shiftZ));
// Speed optimization: perform following checks only if the shifted
// domain touches the bounding box.
Box3D dummyIntersection;
if (intersect(shiftedEnvelope, sparseBlock.getBoundingBox(), dummyIntersection)) {
neighbors.clear();
sparseBlock.findNeighbors(shiftedBulk, envelopeWidth, neighbors);
// Check overlap with each existing block in the neighborhood, including with the newly added one.
for (pluint iNeighbor=0; iNeighbor<neighbors.size(); ++iNeighbor) {
plint neighborId = neighbors[iNeighbor];
SmartBulk3D neighborBulk(sparseBlock, envelopeWidth, neighborId);
// Does the envelope of the shifted new block overlap with the bulk of a previous
// block? If yes, add an overlap, in which the previous block has the "original
// position", and the new block has the "overlap position".
if (intersect(neighborBulk.getBulk(), shiftedEnvelope, intersection)) {
PeriodicOverlap3D overlap (
Overlap3D(neighborId, blockId, intersection, shiftX, shiftY, shiftZ),
dx, dy, dz );
periodicOverlaps.push_back(overlap);
periodicOverlapWithRemoteData.push_back(overlap);
}
// Does the bulk of the shifted new block overlap with the envelope of a previous
// block? If yes, add an overlap, in which the new block has the "original position",
// and the previous block has the "overlap position".
// If we are in the situation in which the newly added block is periodic with itself,
// this step must be skipped, because otherwise the overlap is counted twice.
if (!(neighborId==blockId) &&
intersect(shiftedBulk, neighborBulk.computeEnvelope(), intersection))
{
intersection = intersection.shift(-shiftX,-shiftY, -shiftZ);
periodicOverlaps.push_back (
PeriodicOverlap3D (
Overlap3D(blockId, neighborId, intersection, -shiftX, -shiftY, -shiftZ),
-dx, -dy, -dz ) );
}
}
}
}
}
}
}
}
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);
}
void MultiContainerBlock3D::allocateBlocks()
{
for (pluint iBlock=0; iBlock<this->getLocalInfo().getBlocks().size(); ++iBlock)
{
plint blockId = this->getLocalInfo().getBlocks()[iBlock];
SmartBulk3D bulk(this->getMultiBlockManagement(), blockId);
Box3D envelope = bulk.computeEnvelope();
AtomicContainerBlock3D* newBlock =
new AtomicContainerBlock3D (
envelope.getNx(), envelope.getNy(), envelope.getNz() );
newBlock -> setLocation(Dot3D(envelope.x0, envelope.y0, envelope.z0));
blocks[blockId] = newBlock;
}
}
void LocalMultiBlockInfo3D::computeAllPeriodicOverlaps (
SparseBlockStructure3D const& sparseBlock )
{
for (pluint iBlock=0; iBlock<myBlocks.size(); ++iBlock) {
plint blockId = myBlocks[iBlock];
Box3D bulk;
sparseBlock.getBulk(blockId, bulk);
// Speed optimization: execute the test for periodicity
// only for bulk-domains which touch the bounding box.
if (!contained (
bulk.enlarge(1), sparseBlock.getBoundingBox() ) )
{
computePeriodicOverlaps(sparseBlock, blockId);
}
}
}
SparseBlockStructure3D createRegularDistribution3D (
Box3D const& domain, plint numProc)
{
std::vector<plint> repartition = algorithm::evenRepartition(numProc, 3);
std::vector<plint> newRepartition(3);
if(domain.getNx()>domain.getNy()) { // nx>ny
if(domain.getNx()>domain.getNz()) { // nx>nz
newRepartition[0] = repartition[0];
if (domain.getNy()>domain.getNz()) { // ny>nz
newRepartition[1] = repartition[1];
newRepartition[2] = repartition[2];
}
else { // nz>ny
newRepartition[1] = repartition[2];
newRepartition[2] = repartition[1];
}
}
else { // nz>nx
newRepartition[2] = repartition[0];
newRepartition[1] = repartition[2];
newRepartition[0] = repartition[1];
}
}
else { // ny>nx
if (domain.getNy()>domain.getNz()) { // ny>nz
newRepartition[1] = repartition[0];
if (domain.getNx()>domain.getNz()) { // nx>nz
newRepartition[0] = repartition[1];
newRepartition[2] = repartition[2];
}
else { // nz>nx
newRepartition[0] = repartition[2];
newRepartition[2] = repartition[1];
}
}
else { // nz>ny
newRepartition[2] = repartition[0];
newRepartition[1] = repartition[1];
newRepartition[0] = repartition[2];
}
}
return createRegularDistribution3D (
domain,
newRepartition[0], newRepartition[1], newRepartition[2] );
}
plint Parallelizer3D::computeCost(std::vector<std::vector<Box3D> > const& originalBlocks, Box3D box){
plint totalCost = 0;
plint numLevels = originalBlocks.size();
for (plint iLevel=(plint)originalBlocks.size()-1; iLevel>=0; --iLevel){
// convert the box to the current level
Box3D levelBox = global::getDefaultMultiScaleManager().scaleBox(box,iLevel-(numLevels-1));
for (pluint iComp=0; iComp<originalBlocks[iLevel].size(); ++iComp){
Box3D currentBox;
if (intersect(originalBlocks[iLevel][iComp], levelBox, currentBox)){
plint volume = currentBox.getNx()*currentBox.getNy()*currentBox.getNz();
totalCost += (plint) util::twoToThePower(iLevel) * volume;
}
}
}
return totalCost;
}
SparseBlockStructure3D createRegularDistribution3D (
Box3D const& domain, plint numBlocksX, plint numBlocksY, plint numBlocksZ )
{
SparseBlockStructure3D dataGeometry(domain);
plint posX = domain.x0;
for (plint iBlockX=0; iBlockX<numBlocksX; ++iBlockX) {
plint lx = domain.getNx() / numBlocksX;
if (iBlockX < domain.getNx()%numBlocksX) ++lx;
plint posY = domain.y0;
for (plint iBlockY=0; iBlockY<numBlocksY; ++iBlockY) {
plint ly = domain.getNy() / numBlocksY;
if (iBlockY < domain.getNy()%numBlocksY) ++ly;
plint posZ = domain.z0;
for (plint iBlockZ=0; iBlockZ<numBlocksZ; ++iBlockZ) {
plint lz = domain.getNz() / numBlocksZ;
if (iBlockZ < domain.getNz()%numBlocksZ) ++lz;
dataGeometry.addBlock (
Box3D(posX, posX+lx-1, posY, posY+ly-1, posZ, posZ+lz-1),
dataGeometry.nextIncrementalId() );
posZ += lz;
}
posY += ly;
}
posX += lx;
}
return dataGeometry;
}
bool Box3D::intersectsApprox(const Box3D& b) const
{
Box3D temp;
AABB3D aabb_temp, aabb_temp2;
//make temp localized
temp.dims = b.dims;
toLocal(b.origin, temp.origin);
toLocalReorient(b.xbasis, temp.xbasis);
toLocalReorient(b.ybasis, temp.ybasis);
toLocalReorient(b.zbasis, temp.zbasis);
temp.getAABB(aabb_temp);
aabb_temp2.bmin.setZero();
aabb_temp2.bmax = dims;
if(!aabb_temp2.intersects(aabb_temp))
return false;
temp.dims = dims;
b.toLocal(origin, temp.origin);
b.toLocalReorient(xbasis, temp.xbasis);
b.toLocalReorient(ybasis, temp.ybasis);
b.toLocalReorient(zbasis, temp.zbasis);
temp.getAABB(aabb_temp);
aabb_temp2.bmax = b.dims;
if(!aabb_temp2.intersects(aabb_temp))
return false;
return true;
}
void Octree::BoxLookup(const Box3D& b,vector<int>& nodeIndices) const
{
list<int> q;
q.push_back(0);
while(!q.empty()) {
int n=q.front(); q.pop_front();
if(!b.intersects(nodes[n].bb)) continue;
if(IsLeaf(nodes[n]))
nodeIndices.push_back(n);
else {
for(int i=0;i<8;i++)
q.push_back(nodes[n].childIndices[i]);
}
}
}
void OctreePointSet::BoxQuery(const Box3D& b,vector<Vector3>& points,vector<int>& ids) const
{
points.resize(0);
ids.resize(0);
vector<int> boxnodes;
BoxLookup(b,boxnodes);
for(size_t i=0;i<boxnodes.size();i++) {
const vector<Vector3>& pts = pointLists[boxnodes[i]];
const vector<int>& bids = idLists[boxnodes[i]];
for(size_t k=0;k<pts.size();k++)
if(b.contains(pts[k])) {
points.push_back(pts[k]);
ids.push_back(bids[k]);
}
}
}
bool ClipLine(const Vector3& x, const Vector3& v, const Box3D& b, Real& u1, Real& u2)
{
Vector3 x2, v2;
b.toLocal(x, x2);
//for each face, p is dot(v, normal), q is signed dist to plane (dot(v,normal)-offset)
//normal order: (-1,0,0), (1,0,0), (0,-1,0), (0,1,0), (0,0,-1), (0,0,1)
//offset order: 0, dims.x, 0, dims.y, 0, dims.z
v2.x=dot(v,b.xbasis);
if(ClipLine1D(-x2.x, -v2.x, u1,u2) && ClipLine1D(x2.x-b.dims.x, v2.x, u1,u2)) {
v2.y=dot(v,b.ybasis);
if(ClipLine1D(-x2.y, -v2.y, u1,u2) && ClipLine1D(x2.y-b.dims.y, v2.y, u1,u2)) {
v2.z=dot(v,b.zbasis);
if(ClipLine1D(-x2.z,-v2.z, u1,u2) && ClipLine1D(x2.z-b.dims.z, v2.z, u1,u2)) {
return true;
}
}
}
return false;
}
void Box3D::includeBox(Box3D b)
{
if (_valid) {
_min = Point3(std::min(_min.x(), b.min().x()),
std::min(_min.y(), b.min().y()),
std::min(_min.z(), b.min().z()));
_max = Point3(std::max(_max.x(), b.max().x()),
std::max(_max.y(), b.max().y()),
std::max(_max.z(), b.max().z()));
} else {
_min = b.min();
_max = b.max();
_valid = true;
}
}
void GetBB(const CollisionPointCloud& pc,Box3D& b)
{
b.setTransformed(pc.bblocal,pc.currentTransform);
}
void RobotTestBackend::RenderWorld()
{
ViewRobot& viewRobot = world->robotViews[0];
//WorldViewProgram::RenderWorld();
glDisable(GL_LIGHTING);
drawCoords(0.1);
glEnable(GL_LIGHTING);
for(size_t i=0;i<world->terrains.size();i++)
world->terrains[i]->DrawGL();
for(size_t i=0;i<world->rigidObjects.size();i++)
world->rigidObjects[i]->DrawGL();
if(draw_sensors) {
if(robotSensors.sensors.empty()) {
robotSensors.MakeDefault(robot);
}
for(size_t i=0;i<robotSensors.sensors.size();i++) {
vector<double> measurements;
if(0 == strcmp(robotSensors.sensors[i]->Type(),"CameraSensor")) {
robotSensors.sensors[i]->SimulateKinematic(*robot,*world);
robotSensors.sensors[i]->GetMeasurements(measurements);
}
robotSensors.sensors[i]->DrawGL(*robot,measurements);
}
}
if(draw_geom) {
//set the robot colors
GLColor highlight(1,1,0);
GLColor driven(1,0.5,0);
GLColor colliding(1,0,0);
GLColor blue(0,0,1);
viewRobot.RestoreAppearance();
viewRobot.PushAppearance();
for(size_t i=0;i<robot->links.size();i++) {
if(self_colliding[i]) viewRobot.SetColor(i,colliding);
if((int)i == cur_link)
viewRobot.SetColor(i,highlight);
else if(cur_driver >= 0 && cur_driver < (int)robot->drivers.size() &&
robot->DoesDriverAffect(cur_driver,i))
viewRobot.SetColor(i,driven);
if(draw_self_collision_tests) {
//draw a little blue
if(robot->selfCollisions(i,cur_link) || robot->selfCollisions(cur_link,i) ) {
GLDraw::GeometryAppearance &app = viewRobot.Appearance(i);
app.ModulateColor(blue,0.5);
}
}
}
//this will set the hover colors
allWidgets.DrawGL(viewport);
//viewRobot.Draw();
viewRobot.PopAppearance();
}
else {
for(size_t i=0;i<robotWidgets.size();i++)
robotWidgets[i].linkPoser.draw = 0;
allWidgets.DrawGL(viewport);
for(size_t i=0;i<robotWidgets.size();i++)
robotWidgets[i].linkPoser.draw = 1;
}
if(draw_com) {
viewRobot.DrawCenterOfMass();
for(size_t i=0;i<robot->links.size();i++)
viewRobot.DrawLinkCenterOfMass(i,0.01);
}
if(draw_frame) {
viewRobot.DrawLinkFrames();
glDisable(GL_DEPTH_TEST);
glPushMatrix();
glMultMatrix((Matrix4)robot->links[cur_link].T_World);
drawCoords(0.2);
glPopMatrix();
glEnable(GL_DEPTH_TEST);
}
if(draw_self_collision_tests) {
glDisable(GL_LIGHTING);
glLineWidth(2.0);
glColor3f(1,0,0);
glBegin(GL_LINES);
for(size_t i=0;i<robot->links.size();i++) {
Vector3 comi = robot->links[i].T_World*robot->links[i].com;
for(size_t j=0;j<robot->links.size();j++) {
if(robot->selfCollisions(i,j)) {
Vector3 comj = robot->links[j].T_World*robot->links[j].com;
glVertex3v(comi);
glVertex3v(comj);
}
}
}
glEnd();
}
if(draw_bbs) {
for(size_t j=0;j<robot->geometry.size();j++) {
if(robot->IsGeometryEmpty(j)) continue;
Box3D bbox = robot->geometry[j]->GetBB();
Matrix4 basis;
bbox.getBasis(basis);
glColor3f(1,0,0);
drawOrientedWireBox(bbox.dims.x,bbox.dims.y,bbox.dims.z,basis);
}
}
}
Box3D SmartBulk3D::toLocal(Box3D const& coord) const
{
return Box3D( coord.shift(-bulk.x0+envelopeWidth, -bulk.y0+envelopeWidth,
-bulk.z0+envelopeWidth) );
}
virtual void RenderWorld()
{
if(env) {
glEnable(GL_LIGHTING);
viewEnv.Draw(env);
glDisable(GL_LIGHTING);
}
//drawCoords(0.1);
//viewRobot.DrawJointCoords();
if(draw_geom) {
//set the robot colors
GLColor hover(0.75,0.75,0);
GLColor highlight(1,1,0);
GLColor driven(1,0.5,0);
GLColor colliding(1,0,0);
viewRobot.SetGrey();
for(size_t i=0;i<robot->links.size();i++) {
if(self_colliding[i]) viewRobot.SetColor(i,colliding);
if((int)i == hoverLink)
viewRobot.SetColor(i,hover);
else if((int)i == cur_link)
viewRobot.SetColor(i,highlight);
else if(cur_driver >= 0 && cur_driver < (int)robot->drivers.size() &&
robot->DoesDriverAffect(cur_driver,i))
viewRobot.SetColor(i,driven);
}
viewRobot.Draw();
}
if(draw_com) {
viewRobot.DrawCenterOfMass();
for(size_t i=0;i<robot->links.size();i++)
viewRobot.DrawLinkCenterOfMass(i,0.01);
}
if(draw_frame) {
viewRobot.DrawLinkFrames();
glDisable(GL_DEPTH_TEST);
glPushMatrix();
glMultMatrix((Matrix4)robot->links[cur_link].T_World);
drawCoords(0.2);
glPopMatrix();
glEnable(GL_DEPTH_TEST);
}
if(draw_self_collision_tests) {
glDisable(GL_LIGHTING);
glLineWidth(2.0);
glColor3f(1,0,0);
glBegin(GL_LINES);
for(size_t i=0;i<robot->links.size();i++) {
Vector3 comi = robot->links[i].T_World*robot->links[i].com;
for(size_t j=0;j<robot->links.size();j++) {
if(robot->selfCollisions(i,j)) {
Vector3 comj = robot->links[j].T_World*robot->links[j].com;
glVertex3v(comi);
glVertex3v(comj);
}
}
}
glEnd();
}
if(draw_bbs) {
for(size_t j=0;j<robot->geometry.size();j++) {
if(robot->geometry[j].Empty()) continue;
Box3D bbox = robot->geometry[j].GetBB();
Matrix4 basis;
bbox.getBasis(basis);
glColor3f(1,0,0);
drawOrientedWireBox(bbox.dims.x,bbox.dims.y,bbox.dims.z,basis);
}
}
if(!poseGoals.empty()) {
glPolygonOffset(0,-1000);
glEnable(GL_POLYGON_OFFSET_FILL);
for(size_t i=0;i<poseGoals.size();i++) {
Vector3 curpos = robot->links[poseGoals[i].link].T_World*poseGoals[i].localPosition;
Vector3 despos = poseGoals[i].endPosition;
glDisable(GL_LIGHTING);
glColor3f(1,0,0);
glLineWidth(5.0);
glBegin(GL_LINES);
glVertex3v(curpos);
glVertex3v(despos);
glEnd();
glLineWidth(1.0);
poseWidgets[i].DrawGL(viewport);
float color2[4] = {1,0.5,0,1};
glMaterialfv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,color2);
glPushMatrix();
if(poseGoals[i].rotConstraint == IKGoal::RotFixed) {
RigidTransform T;
poseGoals[i].GetFixedGoalTransform(T);
glMultMatrix(Matrix4(T));
drawBox(0.04,0.04,0.04);
}
else {
glTranslate(despos);
drawSphere(0.02,16,8);
}
glPopMatrix();
}
glDisable(GL_POLYGON_OFFSET_FILL);
}
}
