int main(void)
{
setClock();
lcdPortConfig();
lcdInit();
clearLCD(black);
while(1)
{
if(boxes._1)
makeOpenBox(220,80,yellow);
else
makeBox(220,80,yellow);
if(boxes._2)
makeOpenBox(120,80,green);
else
makeBox(120,80,green);
if(boxes._3)
makeOpenBox(20,80,red);
else
makeBox(20,80,red);
}
}
// Add a new block
void addBlock( const geometry_msgs::Pose pose, int n, bool active, std::string link)
{
InteractiveMarker marker;
marker.header.frame_id = link;
marker.pose = pose;
marker.scale = block_size;
std::stringstream conv;
conv << n;
conv.str();
marker.name = "block" + conv.str();
InteractiveMarkerControl control;
control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 1;
control.orientation.z = 0;
control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
if (active)
marker.controls.push_back( control );
control.markers.push_back( makeBox(marker, .5, .5, .5) );
control.always_visible = true;
marker.controls.push_back( control );
interactive_m_server_.insert( marker );
interactive_m_server_.setCallback( marker.name, boost::bind( &BlockLogicServer::feedbackCb, this, _1 ));
}
void draw_models()
{
glm::vec2 midScreen( 0.5f*proto->getWindowWidth(), 0.5f*proto->getWindowHeight() );
glm::vec2 mouse_coords( (float)proto->getMouseX(), (float)proto->getMouseY() );
poly shape1 = makeBox( midScreen, 60, 80 );
//poly shape1 = makePoly( midScreen, 50.f, 7 );
poly shape2 = makePoly( mouse_coords, 25.f, 6 );
vertexList numIsects = poly_poly(shape1, shape2);
if ( numIsects.size() > 0 ) {
proto->setColor(1.f, 0.f, 0.f); // no seperating axis
for (auto it=numIsects.begin(); it!=numIsects.end(); ++it){
auto v = *it;
proto->drawCircle( v.x, v.y, 3 );
}
} else {
proto->setColor( 0.f, 1.f, 0.f );
}
glm::vec2 mouseCoord( (float)proto->getMouseX(), (float)proto->getMouseY() );
if ( containsv(shape1, mouseCoord) ) {
proto->drawCircle( mouseCoord.x, mouseCoord.y, -3 );
}
shape1.draw( glm::vec2(0.f) );
shape2.draw( glm::vec2(0.f) );
}
void initializeMarker() {
// create an interactive marker server on the topic namespace simple_marker
server.reset( new interactive_markers::InteractiveMarkerServer("fake_people_finder","",false) );
// create an interactive marker for our server
visualization_msgs::InteractiveMarker int_marker;
int_marker.header.frame_id = "/map";
int_marker.name = "fake_person";
int_marker.description = "\t\t\t\tFake Person\n(slide me somewhere in the map!)";
int_marker.pose.position.x = 0;
int_marker.pose.position.y = -7.5;
visualization_msgs::InteractiveMarkerControl control;
control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 1;
control.orientation.z = 0;
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
int_marker.controls.push_back(control);
// make a box which also moves in the plane
control.markers.push_back( makeBox(int_marker) );
control.always_visible = true;
int_marker.controls.push_back(control);
// we want to use our special callback function
server->insert(int_marker);
server->setCallback(int_marker.name, &processFeedback);
// set different callback for POSE_UPDATE feedback
//server->setCallback(int_marker.name, &alignMarker, visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE );
// 'commit' changes and send to all clients
server->applyChanges();
}
void blankOut(WORD startPosition, WORD endPosition){
WORD i = 0;
struct Vector2* startPxCoordinate;
struct Vector2* boxSize;
WORD numLines = (endPosition/WIDTH) - (startPosition/WIDTH);
if(startPosition > endPosition)
swap(startPosition, endPosition);
for(; i < numLines;i++){
startPxCoordinate = getPixelCoordinate(startPosition);
if(endPosition < startOfNextLine(startPosition)){
boxSize = new_Vector2
((getPixelXCoordinate(endPosition)+CHAR_WIDTH)-startPxCoordinate->x
,CHAR_HEIGHT
);
} else {
boxSize = new_Vector2
((getPixelXCoordinate((startOfNextLine(startPosition)-1))+CHAR_WIDTH)-startPxCoordinate->x
,CHAR_HEIGHT
);
startPosition = startOfNextLine(startPosition);
}
makeBox(startPxCoordinate, boxSize, Screen.font.background);
free(startPxCoordinate);
free(boxSize);
}
}
void create3DObjects(void)
{
// Make Object Nodes
NodeRefPtr ExampleTorusGeo = makeTorus(90, 270, 16, 16);
NodeRefPtr ExampleConeGeo = makeCone(150, 50, 16, true, true);
NodeRefPtr ExampleSphereGeo = makeSphere(4, 100);
NodeRefPtr ExampleBoxGeo = makeBox(100, 100, 100, 1, 1, 1);
// AssignTextures
dynamic_cast<Geometry*>(ExampleConeGeo->getCore())->setMaterial(createBlueMaterial());
dynamic_cast<Geometry*>(ExampleSphereGeo->getCore())->setMaterial(createRedMaterial());
dynamic_cast<Geometry*>(ExampleBoxGeo->getCore())->setMaterial(createGreenMaterial());
// Preform transformations on them
Matrix mat;
// On Torus
mat.setTranslate(0.0,100.0,-200.0);
TransformRefPtr TorusTranCore = Transform::create();
TorusTranCore->setMatrix(mat);
ExampleTorus = Node::create();
ExampleTorus->setCore(TorusTranCore);
ExampleTorus->addChild(ExampleTorusGeo);
// On Sphere
mat.setTranslate(250.0,0.0,0.0);
TransformRefPtr SphereTranCore = Transform::create();
SphereTranCore->setMatrix(mat);
ExampleSphere = Node::create();
ExampleSphere->setCore(SphereTranCore);
ExampleSphere->addChild(ExampleSphereGeo);
// On Cone
mat.setTranslate(0.0,0.0,-250.0);
TransformRefPtr ConeTranCore = Transform::create();
ConeTranCore->setMatrix(mat);
ExampleCone = Node::create();
ExampleCone->setCore(ConeTranCore);
ExampleCone->addChild(ExampleConeGeo);
// On Box
mat.setTranslate(250.0,250.0,0.0);
TransformRefPtr ExampleBoxTranCore = Transform::create();
ExampleBoxTranCore->setMatrix(mat);
ExampleBox = Node::create();
ExampleBox->setCore(ExampleBoxTranCore);
ExampleBox->addChild(ExampleBoxGeo);
}
void MarkerController::makeViewFacingMarker(){
int_marker.header.frame_id = "/map";
int_marker.header.stamp = ros::Time::now();
int_marker.pose.position.x = 0;
int_marker.pose.position.y = 0;
int_marker.pose.position.z = 0;
int_marker.scale = 1;
int_marker.name = marker_name;
int_marker.description = "Goal selection " + marker_name;
InteractiveMarkerControl control;
control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
control.independent_marker_orientation = true;
control.name = "move";
control.markers.push_back( makeBox(int_marker) );
control.always_visible = true;
int_marker.controls.push_back(control);
control.interaction_mode = InteractiveMarkerControl::MENU;
control.name = "menu";
int_marker.controls.push_back(control);
server->insert(int_marker);
server->setCallback(int_marker.name, boost::bind(&MarkerController::processFeedback,this,_1));
menu_handler.apply(*server, int_marker.name);
}
InteractiveMarkerControl& MarkerController::makeBoxControl(InteractiveMarker& msg){
InteractiveMarkerControl control;
control.always_visible = true;
control.markers.push_back( makeBox(msg) );
msg.controls.push_back( control );
return msg.controls.back();
}
//===========================================================================
CellDivision::CellDivision(vector<ftSurface*> faces, vector<int> n)
//===========================================================================
{
dim_ = (int)n.size();
ncells_ = n;
faces_ = faces;
makeBox();
constructCells();
}
virtual void started(const DialogEventPtr e)
{
if(DialogPtr::dcast(e->getSource()) == BDialogChildB)
{
NodePtr s = makeSphere(1,2);
beginEditCP(scene, Node::ChildrenFieldMask);
while(scene->getNChildren() > 0)
{
scene->subChild(scene->getNChildren()-1);
}
scene->addChild(s);
endEditCP(scene, Node::ChildrenFieldMask);
}
if(DialogPtr::dcast(e->getSource()) == BDialogChildA)
{
NodePtr s = makeBox(3,3,3,2,2,2);
beginEditCP(scene, Node::ChildrenFieldMask);
while(scene->getNChildren() > 0)
{
scene->subChild(scene->getNChildren()-1);
}
scene->addChild(s);
endEditCP(scene, Node::ChildrenFieldMask);
}
if(DialogPtr::dcast(e->getSource()) == ADialogChildA)
{
beginEditCP(MainInternalWindowBackground, ColorLayer::ColorFieldMask);
MainInternalWindowBackground->setColor(Color4f(0.0,0.0,1.0,0.5));
endEditCP(MainInternalWindowBackground, ColorLayer::ColorFieldMask);
}
if(DialogPtr::dcast(e->getSource()) == ADialogChildB)
{
beginEditCP(MainInternalWindowBackground, ColorLayer::ColorFieldMask);
MainInternalWindowBackground->setColor(Color4f(1.0,0.0,0.0,0.5));
endEditCP(MainInternalWindowBackground, ColorLayer::ColorFieldMask);
}
if(DialogPtr::dcast(e->getSource()) == End1 || DialogPtr::dcast(e->getSource()) == End2 || DialogPtr::dcast(e->getSource()) == End3 || DialogPtr::dcast(e->getSource()) == End4 )
{
TutorialWindowEventProducer->closeWindow();
}
if(DialogPtr::dcast(e->getSource()) == Restart1 || DialogPtr::dcast(e->getSource()) == Restart2 || DialogPtr::dcast(e->getSource()) == Restart3 || DialogPtr::dcast(e->getSource()) == Restart4)
{
beginEditCP(MainInternalWindowBackground, ColorLayer::ColorFieldMask);
MainInternalWindowBackground->setColor(Color4f(1.0,1.0,1.0,0.5));
endEditCP(MainInternalWindowBackground, ColorLayer::ColorFieldMask);
NodePtr s = makeTorus(.5, 2, 16, 16);
beginEditCP(scene, Node::ChildrenFieldMask);
while(scene->getNChildren() > 0)
{
scene->subChild(scene->getNChildren()-1);
}
scene->addChild(s);
endEditCP(scene, Node::ChildrenFieldMask);
}
}
XmlTypeBox::XmlTypeBox(value::Xml* xml) :
Gtk::Dialog("XML", true, true),
mValue(xml)
{
makeBox();
mBuff->set_text(xml->writeToString());
set_size_request(350, 350);
show_all();
}
//////////////////////////////////////////////////////////////////////////
//! build a box
//////////////////////////////////////////////////////////////////////////
void buildBox(void)
{
Vec3f Lengths((Real32)(rand()%2)+1.0, (Real32)(rand()%2)+1.0, (Real32)(rand()%2)+1.0);
Matrix m;
//create OpenSG mesh
GeometryRefPtr box;
NodeRefPtr boxNode = makeBox(Lengths.x(), Lengths.y(), Lengths.z(), 1, 1, 1);
box = dynamic_cast<Geometry*>(boxNode->getCore());
SimpleMaterialRefPtr box_mat = SimpleMaterial::create();
box_mat->setAmbient(Color3f(0.0,0.0,0.0));
box_mat->setDiffuse(Color3f(0.0,1.0 ,0.0));
box->setMaterial(box_mat);
TransformRefPtr boxTrans;
NodeRefPtr boxTransNode = makeCoredNode<Transform>(&boxTrans);
m.setIdentity();
Real32 randX = (Real32)(rand()%10)-5.0;
Real32 randY = (Real32)(rand()%10)-5.0;
m.setTranslate(randX, randY, 10.0);
boxTrans->setMatrix(m);
//create ODE data
PhysicsBodyRefPtr boxBody = PhysicsBody::create(physicsWorld);
boxBody->setPosition(Vec3f(randX, randY, 10.0));
boxBody->setBoxMass(1.0, Lengths.x(), Lengths.y(), Lengths.z());
//std::cout << "mass: " << boxBody->getMass() << std::endl
//<< "massCenterOfGravity: " << boxBody->getMassCenterOfGravity().x() << ", " << boxBody->getMassCenterOfGravity().y() << ", " << boxBody->getMassCenterOfGravity().z() << std::endl
//<< "massInertiaTensor: " << std::endl
//<< boxBody->getMassInertiaTensor()[0][0] << " "<< boxBody->getMassInertiaTensor()[0][1] << " "<< boxBody->getMassInertiaTensor()[0][2] << " " << boxBody->getMassInertiaTensor()[0][3] << std::endl
//<< boxBody->getMassInertiaTensor()[1][0] << " "<< boxBody->getMassInertiaTensor()[1][1] << " "<< boxBody->getMassInertiaTensor()[1][2] << " " << boxBody->getMassInertiaTensor()[1][3] << std::endl
//<< boxBody->getMassInertiaTensor()[2][0] << " "<< boxBody->getMassInertiaTensor()[2][1] << " "<< boxBody->getMassInertiaTensor()[2][2] << " " << boxBody->getMassInertiaTensor()[2][3] << std::endl
//<< boxBody->getMassInertiaTensor()[3][0] << " "<< boxBody->getMassInertiaTensor()[3][1] << " "<< boxBody->getMassInertiaTensor()[3][2] << " " << boxBody->getMassInertiaTensor()[3][3] << std::endl
//<< std::endl;
PhysicsBoxGeomRefPtr boxGeom = PhysicsBoxGeom::create();
boxGeom->setBody(boxBody);
boxGeom->setSpace(physicsSpace);
boxGeom->setLengths(Lengths);
//add attachments
boxNode->addAttachment(boxGeom);
boxTransNode->addAttachment(boxBody);
boxTransNode->addChild(boxNode);
//add to SceneGraph
spaceGroupNode->addChild(boxTransNode);
commitChanges();
}
//////////////////////////////////////////////////////////////////////////
//! build a box
//////////////////////////////////////////////////////////////////////////
void buildBox(void)
{
Vec3f Lengths(frand()*2.0+0.5, frand()*2.0+0.5, frand()*2.0+0.5);
Matrix m;
//create OpenSG mesh
GeometryPtr box;
NodePtr boxNode = makeBox(Lengths.x(), Lengths.y(), Lengths.z(), 1, 1, 1);
box = GeometryPtr::dcast(boxNode->getCore());
SimpleMaterialPtr box_mat = SimpleMaterial::create();
beginEditCP(box_mat);
box_mat->setAmbient(Color3f(0.0,0.0,0.0));
box_mat->setDiffuse(Color3f(0.0,1.0 ,0.0));
endEditCP(box_mat);
beginEditCP(box, Geometry::MaterialFieldMask);
box->setMaterial(box_mat);
endEditCP(box, Geometry::MaterialFieldMask);
TransformPtr boxTrans;
NodePtr boxTransNode = makeCoredNode<Transform>(&boxTrans);
m.setIdentity();
Real32 randX = frand()*10.0-5.0;
Real32 randY = frand()*10.0-5.0;
m.setTranslate(randX, randY, 10.0);
beginEditCP(boxTrans, Transform::MatrixFieldMask);
boxTrans->setMatrix(m);
endEditCP(boxTrans, Transform::MatrixFieldMask);
//create ODE data
PhysicsBodyPtr boxBody = PhysicsBody::create(physicsWorld);
beginEditCP(boxBody, PhysicsBody::PositionFieldMask);
boxBody->setPosition(Vec3f(randX, randY, 10.0));
endEditCP(boxBody, PhysicsBody::PositionFieldMask);
boxBody->setBoxMass(1.0, Lengths.x(), Lengths.y(), Lengths.z());
PhysicsBoxGeomPtr boxGeom = PhysicsBoxGeom::create();
beginEditCP(boxGeom, PhysicsBoxGeom::BodyFieldMask | PhysicsBoxGeom::SpaceFieldMask | PhysicsBoxGeom::LengthsFieldMask | PhysicsBoxGeom::CategoryBitsFieldMask);
boxGeom->setBody(boxBody);
boxGeom->setSpace(physicsSpace);
boxGeom->setLengths(Lengths);
boxGeom->setCategoryBits(BoxCategory);
endEditCP(boxGeom, PhysicsBoxGeom::BodyFieldMask | PhysicsBoxGeom::SpaceFieldMask | PhysicsBoxGeom::LengthsFieldMask | PhysicsBoxGeom::CategoryBitsFieldMask);
//add attachments
beginEditCP(boxNode, Node::AttachmentsFieldMask);
boxNode->addAttachment(boxGeom);
endEditCP(boxNode, Node::AttachmentsFieldMask);
beginEditCP(boxTransNode, Node::AttachmentsFieldMask | Node::ChildrenFieldMask);
boxTransNode->addAttachment(boxBody);
boxTransNode->addChild(boxNode);
endEditCP(boxTransNode, Node::AttachmentsFieldMask | Node::ChildrenFieldMask);
//add to SceneGraph
beginEditCP(spaceGroupNode, Node::ChildrenFieldMask);
spaceGroupNode->addChild(boxTransNode);
endEditCP(spaceGroupNode, Node::ChildrenFieldMask);
}
XmlTypeBox::XmlTypeBox(std::string& string) :
Gtk::Dialog("XML", true, true),
mValue(0),
mXml(&string)
{
makeBox();
mBuff->set_text(string);
set_size_request(350, 350);
show_all();
}
//===========================================================================
CellDivision::CellDivision(vector<ftSurface*> faces, int n1, int n2)
//===========================================================================
{
dim_ = 2;
ncells_.resize(2);
ncells_[0] = n1;
ncells_[1] = n2;
faces_ = faces;
makeBox();
constructCells();
}
//////////////////////////////////////////////////////////////////////////
//! build a ship
//////////////////////////////////////////////////////////////////////////
PhysicsBodyRefPtr buildShip(Vec3f Dimensions, Pnt3f Position)
{
Real32 Radius(osgMax(Dimensions.x(), Dimensions.y())/2.0f);
Real32 Length(Dimensions.z() - 2.0f*Radius);
Matrix m;
//create OpenSG mesh
GeometryRefPtr box;
NodeRefPtr boxNode = makeBox(Dimensions.x(), Dimensions.y(), Dimensions.z(), 1, 1, 1);
box = dynamic_cast<Geometry*>(boxNode->getCore());
SimpleMaterialRefPtr box_mat = SimpleMaterial::create();
box_mat->setAmbient(Color3f(0.0,0.0,0.0));
box_mat->setDiffuse(Color3f(1.0,1.0 ,0.0));
box->setMaterial(box_mat);
TransformRefPtr boxTrans;
NodeRefPtr boxTransNode = makeCoredNode<Transform>(&boxTrans);
m.setIdentity();
m.setTranslate(Position - Vec3f(0.0f,0.0f,0.5f*Dimensions.z()));
boxTrans->setMatrix(m);
for(UInt32 i(0) ; i<box->getPositions()->size() ; ++i)
{
box->getPositions()->setValue<Pnt3f>(box->getPositions()->getValue<Pnt3f>(i) + Vec3f(0.0,0.0,Dimensions.z()/2.0f),i);
}
//create ODE data
PhysicsBodyRefPtr CapsuleBody = PhysicsBody::create(physicsWorld);
CapsuleBody->setPosition(Vec3f(Position - Vec3f(0.0f,0.0f,0.5f*Dimensions.z())));
CapsuleBody->setLinearDamping(0.01);
CapsuleBody->setMaxAngularSpeed(0.0);
CapsuleBody->setCapsuleMass(1.0,3,Radius, Length);
PhysicsCapsuleGeomRefPtr CapsuleGeom = PhysicsCapsuleGeom::create();
CapsuleGeom->setBody(CapsuleBody);
CapsuleGeom->setOffsetPosition(Vec3f(0.0f,0.0f,0.5f*Dimensions.z()));
CapsuleGeom->setSpace(hashSpace);
CapsuleGeom->setRadius(Radius);
CapsuleGeom->setLength(Length);
//add attachments
boxNode->addAttachment(CapsuleGeom);
boxTransNode->addAttachment(CapsuleBody);
boxTransNode->addChild(boxNode);
//add to SceneGraph
spaceGroupNode->addChild(boxTransNode);
commitChanges();
return CapsuleBody;
}
//////////////////////////////////////////////////////////////////////////
//! build a character
//////////////////////////////////////////////////////////////////////////
PhysicsBodyRefPtr buildCharacter(Vec3f Dimensions,
Pnt3f Position,
Node* const spaceGroupNode,
PhysicsWorld* const physicsWorld,
PhysicsHashSpace* const physicsSpace
)
{
Real32 Radius(osgMax(Dimensions.x(), Dimensions.y())/2.0f);
Real32 Length(Dimensions.z() - 2.0f*Radius);
Matrix m;
//create OpenSG mesh
GeometryRefPtr box;
//NodeRefPtr characterNode = makeBox(Dimensions.x(), Dimensions.y(), Dimensions.z(), 1, 1, 1);
NodeRefPtr characterNode = SceneFileHandler::the()->read("Data/Jack.osb");
if(characterNode == NULL)
{
characterNode = makeBox(Dimensions.x(), Dimensions.y(), Dimensions.z(), 1, 1, 1);
}
box = dynamic_cast<Geometry*>(characterNode->getCore());
TransformRefPtr boxTrans;
NodeRefPtr boxTransNode = makeCoredNode<Transform>(&boxTrans);
m.setIdentity();
m.setTranslate(Position);
boxTrans->setMatrix(m);
//create ODE data
PhysicsBodyRefPtr boxBody = PhysicsBody::create(physicsWorld);
boxBody->setPosition(Vec3f(Position));
//boxBody->setLinearDamping(0.001);
//boxBody->setAngularDamping(0.001);
boxBody->setMaxAngularSpeed(0.0);
boxBody->setCapsuleMass(1.0,3,Radius, Length);
PhysicsCapsuleGeomRefPtr CapsuleGeom = PhysicsCapsuleGeom::create();
CapsuleGeom->setBody(boxBody);
CapsuleGeom->setSpace(physicsSpace);
CapsuleGeom->setRadius(Radius);
CapsuleGeom->setLength(Length);
//add attachments
characterNode->addAttachment(CapsuleGeom);
boxTransNode->addAttachment(boxBody);
boxTransNode->addChild(characterNode);
//add to SceneGraph
spaceGroupNode->addChild(boxTransNode);
commitChanges();
return boxBody;
}
NodeTransitPtr buildScene(void)
{
NodeUnrecPtr geoN = makeBox(1.f, 1.f, 1.f, 1, 1, 1);
ComponentTransformUnrecPtr xform = ComponentTransform::create();
NodeUnrecPtr xformN = makeNodeFor(xform);
setTargetId(xform, "xform0");
gXForm = xform;
NodeUnrecPtr groupN = makeCoredNode<Group>();
xformN->addChild(geoN );
groupN->addChild(xformN);
return NodeTransitPtr(groupN);
}
bool test()
{
glm::vec2 midScreen( 0.5f*proto->getWindowWidth(), 0.5f*proto->getWindowHeight() );
poly box = makeBox( midScreen, 100, 100 );
bool isIn = true;
for ( int i=0; i<2; i++ ){
isIn &= containsv( box, midScreen+glm::vec2( protowizard::sfrand()*4, protowizard::sfrand()*4 ) );
}
bool isIn2 = true;
for ( int i=0; i<2; i++ ){
isIn2 &= containsv( box, glm::vec2( protowizard::sfrand()*4, protowizard::sfrand()*4 ) );
}
bool allGood = isIn==true && isIn2==false;
return allGood;
}
void StigmergyPlanner::makeViewFacingMarker(tf::StampedTransform robot_pose)
{
int_marker.header.frame_id = robot_pose.frame_id_;
int_marker.header.stamp = robot_pose.stamp_;
int_marker.pose.position.x = robot_pose.getOrigin().getX();
int_marker.pose.position.y = robot_pose.getOrigin().getY();
int_marker.pose.position.z = robot_pose.getOrigin().getZ();
int_marker.scale = 1;
int_marker.name = "view_facing";
int_marker.description = "3D Planning pencil";
InteractiveMarkerControl control;
// make a control that rotates around the view axis
//control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
//control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
//control.orientation.w = 1;
//control.name = "rotate";
//int_marker.controls.push_back(control);
// create a box in the center which should not be view facing,
// but move in the camera plane.
control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
control.independent_marker_orientation = true;
control.name = "move";
control.markers.push_back( makeBox(int_marker) );
control.always_visible = true;
int_marker.controls.push_back(control);
control.interaction_mode = InteractiveMarkerControl::MENU;
control.name = "menu";
int_marker.controls.push_back(control);
server->insert(int_marker);
server->setCallback(int_marker.name, boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.apply(*server, int_marker.name);
}
PhysicsBodyUnrecPtr buildBox(const Pnt3f& Position,
const Vec3f& Dimensions,
const Color3f& Color,
Node* const spaceGroupNode,
PhysicsWorld* const physicsWorld,
PhysicsHashSpace* const physicsSpace)
{
Matrix m;
//create OpenSG mesh
GeometryUnrecPtr box;
NodeUnrecPtr boxNode = makeBox(Dimensions.x(), Dimensions.y(), Dimensions.z(), 1, 1, 1);
box = dynamic_cast<Geometry*>(boxNode->getCore());
SimpleMaterialUnrecPtr box_mat = SimpleMaterial::create();
box_mat->setAmbient(Color3f(0.0,0.0,0.0));
box_mat->setDiffuse(Color);
box->setMaterial(box_mat);
TransformUnrecPtr boxTrans;
NodeUnrecPtr boxTransNode = makeCoredNode<Transform>(&boxTrans);
m.setIdentity();
m.setTranslate(Position);
boxTrans->setMatrix(m);
//create ODE data
PhysicsBodyUnrecPtr boxBody = PhysicsBody::create(physicsWorld);
boxBody->setPosition(Vec3f(Position));
boxBody->setBoxMass(1.0,Dimensions.x(), Dimensions.y(), Dimensions.z());
boxBody->setLinearDamping(0.0001);
boxBody->setAngularDamping(0.0001);
PhysicsBoxGeomUnrecPtr boxGeom = PhysicsBoxGeom::create();
boxGeom->setBody(boxBody);
boxGeom->setSpace(physicsSpace);
boxGeom->setLengths(Dimensions);
//add attachments
boxNode->addAttachment(boxGeom);
boxTransNode->addAttachment(boxBody);
boxTransNode->addChild(boxNode);
//add to SceneGraph
spaceGroupNode->addChild(boxTransNode);
return boxBody;
}
int add_pdf_page(struct pdf_info * info, int pagen, unsigned width, unsigned height, int bpp, unsigned dpi)
{
try {
finish_page(info); // any active
info->width = width;
info->height = height;
info->pixel_bytes = bpp/8;
info->line_bytes = (width*info->pixel_bytes);
info->bpp = bpp;
if (info->height > (std::numeric_limits<unsigned>::max() / info->line_bytes)) {
die("Page too big");
}
info->page_data = PointerHolder<Buffer>(new Buffer(info->line_bytes*info->height));
QPDFObjectHandle page = QPDFObjectHandle::parse(
"<<"
" /Type /Page"
" /Resources <<"
" /XObject << >> "
" >>"
" /MediaBox null "
" /Contents null "
">>");
page.replaceKey("/Contents",QPDFObjectHandle::newStream(&info->pdf)); // data will be provided later
// Convert to pdf units
info->page_width=((double)info->width/dpi)*DEFAULT_PDF_UNIT;
info->page_height=((double)info->height/dpi)*DEFAULT_PDF_UNIT;
page.replaceKey("/MediaBox",makeBox(0,0,info->page_width,info->page_height));
info->page = info->pdf.makeIndirectObject(page); // we want to keep a reference
info->pdf.addPage(info->page, false);
} catch (std::bad_alloc &ex) {
die("Unable to allocate page data");
} catch (...) {
return 1;
}
return 0;
}
//--------------------------------------------------------------
void testApp::keyPressed(int key){
switch (key){
case 'k':
clearPendulums();
break;
case 'b':
makeBall();
break;
case 'x':
makeBox();
break;
case 'c':
clearBallsAndBoxes();
break;
}
}
//////////////////////////////////////////////////////////////////////////
//! build a box
//////////////////////////////////////////////////////////////////////////
PhysicsBodyUnrecPtr buildBox(void)
{
Vec3f Lengths((Real32)(rand()%2)+1.0, (Real32)(rand()%2)+1.0, (Real32)(rand()%2)+1.0);
Matrix m;
//create OpenSG mesh
GeometryUnrecPtr box;
NodeUnrecPtr boxNode = makeBox(Lengths.x(), Lengths.y(), Lengths.z(), 1, 1, 1);
box = dynamic_cast<Geometry*>(boxNode->getCore());
SimpleMaterialUnrecPtr box_mat = SimpleMaterial::create();
box_mat->setAmbient(Color3f(0.0,0.0,0.0));
box_mat->setDiffuse(Color3f(0.0,1.0 ,0.0));
box->setMaterial(box_mat);
TransformUnrecPtr boxTrans;
NodeUnrecPtr boxTransNode = makeCoredNode<Transform>(&boxTrans);
m.setIdentity();
Real32 randX = (Real32)(rand()%10)-5.0;
Real32 randY = (Real32)(rand()%10)-5.0;
m.setTranslate(randX, randY, 10.0);
boxTrans->setMatrix(m);
//create ODE data
PhysicsBodyUnrecPtr boxBody = PhysicsBody::create(physicsWorld);
boxBody->setPosition(Vec3f(randX, randY, 10.0));
boxBody->setBoxMass(1.0, Lengths.x(), Lengths.y(), Lengths.z());
PhysicsBoxGeomUnrecPtr boxGeom = PhysicsBoxGeom::create();
boxGeom->setBody(boxBody);
boxGeom->setSpace(physicsSpace);
boxGeom->setLengths(Lengths);
//add attachments
boxNode->addAttachment(boxGeom);
boxTransNode->addAttachment(boxBody);
boxTransNode->addChild(boxNode);
//add to SceneGraph
spaceGroupNode->addChild(boxTransNode);
commitChanges();
return boxBody;
}
NodeTransitPtr createBox(void)
{
// Make Object Nodes
NodeRecPtr ExampleBoxGeo = makeBox(100, 100, 100, 1, 1, 1);
MaterialRecPtr GreenMaterial = createMaterial(Color4f(0.0f,1.0f,0.0f,1.0f));
dynamic_cast<Geometry*>(ExampleBoxGeo->getCore())->setMaterial(GreenMaterial);
Matrix mat;
mat.setTranslate(250.0,250.0,0.0);
TransformRecPtr ExampleBoxTranCore = Transform::create();
ExampleBoxTranCore->setMatrix(mat);
NodeRecPtr ExampleBox = Node::create();
ExampleBox->setCore(ExampleBoxTranCore);
ExampleBox->addChild(ExampleBoxGeo);
ExampleBox->setTravMask(0);
return NodeTransitPtr(ExampleBox);
}
osg::ref_ptr< osg::Group > EnSkyBox::setupSkybox()
{
_textureFilenameMap.clear();
// setup texture side names
_textureFilenameMap.insert( make_pair( osg::TextureCubeMap::POSITIVE_X, _texNames[ 0 ] ) );
_textureFilenameMap.insert( make_pair( osg::TextureCubeMap::NEGATIVE_X, _texNames[ 1 ] ) );
_textureFilenameMap.insert( make_pair( osg::TextureCubeMap::POSITIVE_Y, _texNames[ 2 ] ) );
_textureFilenameMap.insert( make_pair( osg::TextureCubeMap::NEGATIVE_Y, _texNames[ 3 ] ) );
_textureFilenameMap.insert( make_pair( osg::TextureCubeMap::POSITIVE_Z, _texNames[ 4 ] ) );
_textureFilenameMap.insert( make_pair( osg::TextureCubeMap::NEGATIVE_Z, _texNames[ 5 ] ) );
osg::ref_ptr< osg::Group > skyboxgrp = new osg::Group();
skyboxgrp->setName( "_skybox_" );
_p_transformEyePoint = new vrc::gameutils::EyeTransform();
_p_transformEyePoint->setCullingActive( false );
_p_transformEyePoint->addChild( makeBox() );
skyboxgrp->addChild( _p_transformEyePoint );
return skyboxgrp;
}
virtual void segmentActivated(const CaptionEventPtr e)
{
if(segUpdate == 6)
{
NodePtr s = makeSphere(1,2);
beginEditCP(scene, Node::ChildrenFieldMask);
while(scene->getNChildren() > 0)
{
scene->subChild(scene->getNChildren()-1);
}
scene->addChild(s);
endEditCP(scene, Node::ChildrenFieldMask);
}
if(segUpdate == 9)
{
NodePtr s = makeBox(3,3,3,2,2,2);
beginEditCP(scene, Node::ChildrenFieldMask);
while(scene->getNChildren() > 0)
{
scene->subChild(scene->getNChildren()-1);
}
scene->addChild(s);
endEditCP(scene, Node::ChildrenFieldMask);
}
if(segUpdate == 11)
{
beginEditCP(scene, Node::ChildrenFieldMask);
while(scene->getNChildren() > 0)
{
scene->subChild(scene->getNChildren()-1);
}
endEditCP(scene, Node::ChildrenFieldMask);
}
++segUpdate;
std::cout<<"Segment Activated"<<std::endl;
}
// Fills the stack, but only checks a maximum number of maxToCheck points at a time.
// Further calls to this function will continue the expand/check neighbors algorithm.
void GeoBrowse::fillStack(int maxToCheck, int maxToAdd, bool onlyExpand) {
if(maxToAdd < 0) maxToAdd = maxToCheck;
int maxFound = _foundInExp + maxToCheck;
verify(maxToCheck > 0);
verify(maxFound > 0);
verify(_found <= 0x7fffffff); // conversion to int
int maxAdded = static_cast<int>(_found) + maxToAdd;
verify(maxAdded >= 0); // overflow check
bool isNeighbor = _centerPrefix.constrains();
// Starting a box expansion
if (_state == START) {
// Get the very first hash point, if required
if(! isNeighbor)
_prefix = expandStartHash();
if (!BtreeLocation::initial(_descriptor, _params, _min, _max, _prefix)) {
_state = isNeighbor ? DONE_NEIGHBOR : DONE;
} else {
_state = DOING_EXPAND;
_lastPrefix.reset();
}
}
// Doing the actual box expansion
if (_state == DOING_EXPAND) {
while (true) {
// Record the prefix we're actively exploring...
_expPrefix.reset(new GeoHash(_prefix));
// Find points inside this prefix
while (checkAndAdvance(&_min, _prefix, _foundInExp)
&& _foundInExp < maxFound && _found < maxAdded) {}
while (checkAndAdvance(&_max, _prefix, _foundInExp)
&& _foundInExp < maxFound && _found < maxAdded) {}
if(_foundInExp >= maxFound || _found >= maxAdded) return;
// We've searched this prefix fully, remember
_lastPrefix.reset(new GeoHash(_prefix));
// If we've searched the entire space, we're finished.
if (! _prefix.constrains()) {
_state = DONE;
notePrefix();
return;
}
// If we won't fit in the box, and we're not doing a sub-scan, increase the size
if (! fitsInBox(_converter->sizeEdge(_prefix)) && _fringe.size() == 0) {
// If we're still not expanded bigger than the box size, expand again
_prefix = _prefix.up();
continue;
}
// We're done and our size is large enough
_state = DONE_NEIGHBOR;
// Go to the next sub-box, if applicable
if(_fringe.size() > 0) _fringe.pop_back();
// Go to the next neighbor if this was the last sub-search
if(_fringe.size() == 0) _neighbor++;
break;
}
notePrefix();
}
// If we doeighbors
if(onlyExpand) return;
// If we're done expanding the current box...
if(_state == DONE_NEIGHBOR) {
// Iterate to the next neighbor
// Loop is useful for cases where we want to skip over boxes entirely,
// otherwise recursion increments the neighbors.
for (; _neighbor < 9; _neighbor++) {
// If we have no fringe for the neighbor, make sure we have the default fringe
if(_fringe.size() == 0) _fringe.push_back("");
if(! isNeighbor) {
_centerPrefix = _prefix;
_centerBox = makeBox(_centerPrefix);
isNeighbor = true;
}
int i = (_neighbor / 3) - 1;
int j = (_neighbor % 3) - 1;
if ((i == 0 && j == 0) ||
(i < 0 && _centerPrefix.atMinX()) ||
(i > 0 && _centerPrefix.atMaxX()) ||
(j < 0 && _centerPrefix.atMinY()) ||
(j > 0 && _centerPrefix.atMaxY())) {
continue; // main box or wrapped edge
// TODO: We may want to enable wrapping in future, probably best as layer
// on top of this search.
}
// Make sure we've got a reasonable center
verify(_centerPrefix.constrains());
GeoHash _neighborPrefix = _centerPrefix;
_neighborPrefix.move(i, j);
while(_fringe.size() > 0) {
_prefix = _neighborPrefix + _fringe.back();
Box cur(makeBox(_prefix));
double intAmt = intersectsBox(cur);
// No intersection
if(intAmt <= 0) {
_fringe.pop_back();
continue;
} else if(intAmt < 0.5 && _prefix.canRefine()
&& _fringe.back().size() < 4 /* two bits */) {
// Small intersection, refine search
string lastSuffix = _fringe.back();
_fringe.pop_back();
_fringe.push_back(lastSuffix + "00");
_fringe.push_back(lastSuffix + "01");
_fringe.push_back(lastSuffix + "11");
_fringe.push_back(lastSuffix + "10");
continue;
}
// Restart our search from a diff box.
_state = START;
verify(! onlyExpand);
verify(_found <= 0x7fffffff);
fillStack(maxFound - _foundInExp, maxAdded - static_cast<int>(_found));
// When we return from the recursive fillStack call, we'll either have
// checked enough points or be entirely done. Max recurse depth is < 8 *
// 16.
// If we're maxed out on points, return
if(_foundInExp >= maxFound || _found >= maxAdded) {
// Make sure we'll come back to add more points
verify(_state == DOING_EXPAND);
return;
}
// Otherwise we must be finished to return
verify(_state == DONE);
return;
}
}
// Finished with neighbors
_state = DONE;
}
}
void InitScene()
{
Logger::log("Creating Material\n\n");
// Generate a box material
Material *boxMaterial = new Material();
// Load textures for this material
Texture::create(CRATE_PCX,"crate1");
// Apply the textures as layer 0, 1, 2
boxMaterial->bindTexture(0,Texture::getTextureId("crate1"),TEXTURE_DIFF);
// Create the box structure and bind our material to each face
makeBox(boxObj,1.0,boxMaterial);
// Create a UV Mapper
UVMapper myUVMapper;
// Set the projection type to cubic
myUVMapper.setProjection(UV_PROJECTION_CUBIC);
// Match the scale of our box
myUVMapper.setScale(XYZ(1.0,1.0,1.0));
// Apply the UV map to the material we bound to our box object
myUVMapper.apply(boxObj,boxMaterial);
// Now cache the object onto the card for best performance.
boxObj.cache(true);
MeshRenderer tmpShader;
mkLandscape();
//CreateTower(2,4,2,XYZ(-1.5,5,-1.5),XYZ(1,1,1));
// Set up the global ambient factor
// Light::setGlobalAmbient(RGB(0.1,0.1,0.1));
// Set up our light
for (int i = 0; i < NUM_LIGHTS; i++)
{
myLights[i].setType(LIGHT_POINT);
myLights[i].diffuse = RGB((float)rand()/(float)RAND_MAX,(float)rand()/(float)RAND_MAX,(float)rand()/(float)RAND_MAX);
myLights[i].setCutoff(20);
myScene.bind(myLights[i]);
}
// Set up our camera (targeted, looking at 0,0,0)
myCamera.setType(CAMERA_TARGET);
myCamera.position = XYZ(-8.0,8.0,-8.0);
//myCamera.target = XYZ(0,5,0);
targetObj.setPosition(XYZ(0,0,0));
myCamera.bindTarget(&targetObj);
myCamera.setClip(0.01,100);
// Bind the light and camera
myScene.bind(myCamera);
/*
myLightR.position = XYZ(cos(10*2.0)*8.0, 6, sin(10*2.0)*8.0);
myLightG.position = XYZ(cos(10 )*4.0, 6, sin(10 )*4.0);
myLightB.position = XYZ(cos(10*-1.5)*10.0,6, sin(10*-1.5)*10.0);
*/
}
gmi_model* MeshAdaptPUMIDrvr::createSphereInBox(double* boxDim,double*sphereCenter, double radius)
{
sphereRadius = radius;
boxLength = boxDim[0];
boxWidth = boxDim[1];
boxHeight = boxDim[2];
xyz_offset[0] = sphereCenter[0];
xyz_offset[1] = sphereCenter[1];
xyz_offset[2] = sphereCenter[2];
lion_set_verbosity(1);
//create the analytic model
gmi_model* model = gmi_make_analytic();
//add the sphere
makeSphere(model);
//add the box
makeBox(model);
//apf::writeVtkFiles("initialInitial",m);
setParameterization(model,m);
m->verify();
//apf::Field* size_initial = apf::createLagrangeField(m,"size_initial",apf::SCALAR,1);
size_iso = apf::createLagrangeField(m,"proteus_size",apf::SCALAR,1);
apf::MeshIterator* it = m->begin(0);
apf::MeshEntity* ent;
while( (ent = m->iterate(it)) )
{
apf::Vector3 pt;
m->getPoint(ent,0,pt);
if(sqrt( (pt[0]-xyz_offset[0])*(pt[0]-xyz_offset[0])+ (pt[1]-xyz_offset[1])*(pt[1]-xyz_offset[1]) + (pt[2]-xyz_offset[2])*(pt[2]-xyz_offset[2])) < sphereRadius*1.5)
apf::setScalar(size_iso,ent,0,hmin);
else
apf::setScalar(size_iso,ent,0,hmax);
}
m->end(it);
gradeMesh();
ma::Input* in = ma::configure(m,size_iso);
in->maximumIterations = 10;
in->shouldSnap = true;
in->shouldTransferParametric = true;
in->shouldFixShape = true;
in->debugFolder="./debug_fine";
ma::adaptVerbose(in,false);
m->verify();
//apf::writeVtkFiles("initialAdapt",m);
freeField(size_iso);
size_iso = apf::createLagrangeField(m,"proteus_size",apf::SCALAR,1);
it = m->begin(0);
while( (ent = m->iterate(it)) )
{
apf::Vector3 pt;
m->getPoint(ent,0,pt);
if(sqrt( (pt[0]-xyz_offset[0])*(pt[0]-xyz_offset[0])+ (pt[1]-xyz_offset[1])*(pt[1]-xyz_offset[1]) + (pt[2]-xyz_offset[2])*(pt[2]-xyz_offset[2])) < sphereRadius*1.5)
apf::setScalar(size_iso,ent,0,hmin);
else
apf::setScalar(size_iso,ent,0,hmax);
}
m->end(it);
gradeMesh();
in = ma::configure(m,size_iso);
in->maximumIterations = 10;
in->shouldSnap = true;
in->shouldTransferParametric = true;
in->shouldFixShape = true;
in->debugFolder="./debug_fine";
ma::adaptVerbose(in,false);
m->verify();
//apf::writeVtkFiles("initialAdapt2",m);
freeField(size_iso);
return model;
}
