static BasicDemo::DemoApplication* Create()
{
BasicDemo* demo = new BasicDemo;
demo->myinit();
demo->initPhysics();
return demo;
}
virtual void createBodyForCompoundShape(btCompoundShape* compoundTmpShape,bool addConstraint, const btTransform& worldTransform, btScalar mass)
{
btDefaultMotionState* myMotionState= 0;
btVector3 aabbMin,aabbMax;
compoundTmpShape->getAabb(btTransform::getIdentity(),aabbMin,aabbMax);
int numSpheres = compoundTmpShape->getNumChildShapes();
btAssert(numSpheres>0);
if (numSpheres>8)
{
printf("error: exceeded 8 spheres\n");
return;
}
btVector3* positions = new btVector3[numSpheres];
btScalar* radii = new btScalar[numSpheres];
for (int i=0;i<numSpheres;i++)
{
btAssert(compoundTmpShape->getChildShape(i)->getShapeType()== SPHERE_SHAPE_PROXYTYPE);
btSphereShape* sphereShape = (btSphereShape*)compoundTmpShape->getChildShape(i);
radii[i]=sphereShape->getRadius();
positions[i] = compoundTmpShape->getChildTransform(i).getOrigin();
}
btMultiSphereShape* multiSphere = new btMultiSphereShape(positions,radii,numSpheres);
m_demo->addCollisionShape(multiSphere);
btVector3 localInertia(0,0,0);
if (mass)
{
myMotionState = new btDefaultMotionState(worldTransform);
multiSphere->calculateLocalInertia(mass,localInertia);
}
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btRigidBody* body = new btRigidBody(mass,myMotionState,multiSphere,localInertia);
body->setLinearFactor(btVector3(1,1,0));
body->setAngularFactor(btVector3(0,0,1));
body->setWorldTransform(worldTransform);
m_demo->getDynamicsWorld()->addRigidBody(body);
if (addConstraint)
{
btVector3 pivotInA(0,0,0);
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,pivotInA);
m_demo->getDynamicsWorld()->addConstraint(p2p);
}
}
int main(int argc, char* argv[])
{
float dt = 1./120.f;
#ifdef BT_DEBUG
char* name = "Bullet 2 CPU FeatherstoneMultiBodyDemo (Debug build=SLOW)";
#else
char* name = "Bullet 2 CPU FeatherstoneMultiBodyDemo";
#endif
SimpleOpenGL3App* app = new SimpleOpenGL3App(name,1024,768);
app->m_instancingRenderer->setCameraDistance(40);
app->m_instancingRenderer->setCameraPitch(0);
app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0));
app->m_window->setMouseMoveCallback(MyMouseMoveCallback);
app->m_window->setMouseButtonCallback(MyMouseButtonCallback);
BasicDemo* demo = new BasicDemo(app);
demo->initPhysics();
sDemo = demo;
GLint err = glGetError();
assert(err==GL_NO_ERROR);
do
{
GLint err = glGetError();
assert(err==GL_NO_ERROR);
app->m_instancingRenderer->init();
app->m_instancingRenderer->updateCamera();
demo->stepSimulation();
demo->drawObjects();
app->drawGrid(10,0.01);
char bla[1024];
static int frameCount = 0;
frameCount++;
sprintf(bla,"Simulation frame %d", frameCount);
app->drawText(bla,10,10);
app->swapBuffer();
} while (!app->m_window->requestedExit());
demo->exitPhysics();
delete demo;
delete app;
return 0;
}
int main(int argc,char** argv)
{
BasicDemo ccdDemo;
ccdDemo.initPhysics();
int i;
for (i=0;i<5;i++)
ccdDemo.clientMoveAndDisplay();
ccdDemo.exitPhysics();
return 0;
}
int main(int argc,char** argv)
{
myargc = argc;
myargv = argv;
BasicDemo ccdDemo;
fprintf(stderr,"after declaration");
ccdDemo.myinit();
ccdDemo.initPhysics();
return glutmain(argc, argv,1024,600,"Bullet Physics Demo. http://bulletphysics.org",&ccdDemo);
//default glut doesn't return from mainloop
return 0;
}
int main(int argc,char** argv)
{
GLDebugDrawer gDebugDrawer;
///testing the btHashMap
btHashMap<btHashKey<OurValue>,OurValue> map;
OurValue value1(btVector3(2,3,4));
btHashKey<OurValue> key1(value1.getUid());
map.insert(key1,value1);
OurValue value2(btVector3(5,6,7));
btHashKey<OurValue> key2(value2.getUid());
map.insert(key2,value2);
{
OurValue value3(btVector3(7,8,9));
btHashKey<OurValue> key3(value3.getUid());
map.insert(key3,value3);
}
map.remove(key2);
const OurValue* ourPtr = map.find(key1);
for (int i=0;i<map.size();i++)
{
OurValue* tmp = map.getAtIndex(i);
//printf("tmp value=%f,%f,%f\n",tmp->m_position.getX(),tmp->m_position.getY(),tmp->m_position.getZ());
}
BasicDemo ccdDemo;
ccdDemo.initPhysics();
ccdDemo.getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
#ifdef CHECK_MEMORY_LEAKS
ccdDemo.exitPhysics();
#else
return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://bullet.sf.net",&ccdDemo);
#endif
//default glut doesn't return from mainloop
return 0;
}
int main(int argc, char* argv[])
{
#ifdef __APPLE__
MacOpenGLWindow* window = new MacOpenGLWindow();
#else
Win32OpenGLWindow* window = new Win32OpenGLWindow();
#endif
btgWindowConstructionInfo wci(g_OpenGLWidth,g_OpenGLHeight);
wci.m_openglVersion = 2;
window->createWindow(wci);
window->setWindowTitle("MyTest");
#ifdef _WIN32
glewInit();
#endif
window->startRendering();
float color[4] = {1,1,1,1};
// prim.drawRect(0,0,200,200,color);
window->endRendering();
window->setKeyboardCallback(MyKeyboardCallback);
{
BasicDemo* demo = new BasicDemo;
demo->myinit();
demo->initPhysics();
do
{
window->startRendering();
demo->clientMoveAndDisplay();
render.init();
render.renderPhysicsWorld(demo->getDynamicsWorld());
window->endRendering();
} while (!window->requestedExit());
demo->exitPhysics();
delete demo;
}
window->closeWindow();
delete window;
return 0;
}
void basicBayes::runRealWorld(){
bool realWorld = false;
if (realWorld == false){
//When you want to use a simulation for the real world
int realModel = 3;
std::cout << "this is the real world" << std::endl;
if (realModel == 0){
BasicDemo world;
realCurrentState_ = world.runSimulation(realPrevState_,realAction_);
}
else if (realModel == 1){
realCurrentState_ = realPrevState_;
}
else if (realModel == 2){
BasicDemoRev world;
std::vector<double> temp;
temp.push_back(atan2(realPrevState_[1],realPrevState_[0]-0.44));
realCurrentState_ = world.runSimulation(temp,realAction_);
}
else if (realModel == 3){
BasicDemoPris world;
std::vector<double> temp;
temp.push_back(realPrevState_[0]);
realCurrentState_ = world.runSimulation(temp,realAction_);
}
}
//When you want to use the robot for the real world
//translation of the coordinate systems is required
//bullet: +x forward, +y up, +z to the right, realAction_, realCurrentState_
//pr2: +x forward, +y to the left, +z up, translatedAction, translatedCurrentState
else if (realWorld == true){
std::vector<double> translatedAction;
translatedAction.push_back(realAction_[0]); //b to pr2: x+ to x+
translatedAction.push_back(-realAction_[1]); // +z to +y
//translatedAction.push_back(0.0);
//translatedAction.push_back(0.0);
translatedAction.push_back(0.0); //this is just +z for the pr2 which isn't read anyways
sendRequest(translatedAction);
//realCurrentState_.clear();
//realCurrentState_.shrink_to_fit();
std::vector<double> translatedCurrentState;
std::cout << "Now, waiting for a response from the robot" << std::endl;
while(!getResponse(translatedCurrentState)){
//std::cout << "waiting for a response from the robot" << std::endl;
}
realCurrentState_[0] = translatedCurrentState[0]; //pr2 to b: x+ to x+
realCurrentState_[1] = -translatedCurrentState[1]; // +y to +z
std::cout << "observation from robot: " << realCurrentState_[0] << " , " << realCurrentState_[1] << std::endl;
}
}
std::vector<double> basicBayes::stateTransition(std::vector<double> prevState, std::vector<double> action){
//this funciton is problem specific
//make this do something eventually
std::vector<double> nextState;
double sum = std::accumulate(action.begin(),action.end(),(double) 0);
if (sum == -100.0) {
//Didn't do anything kind of this has to change
nextState = prevState;
}
else {
//this is saying there is no probability to transition between models
//prevState[0] is the model. 0.0 is free, 1.0 is fixed, 2.0 is revolute, 3.0 is prismatic
//WOM means without model.
std::vector<double> prevStateWOM;
prevStateWOM.assign(prevState.begin()+1,prevState.end()); //state without the model which is always the first entry in the state
std::vector<double> tempWorldState;
if (prevState[0]==0.0) {
//model: free
BasicDemo world;
tempWorldState = world.runSimulation(prevStateWOM,action);
nextState.push_back(0.0); //this is saying you end up in the same state
for(size_t j=0;j<tempWorldState.size();j++){
nextState.push_back(tempWorldState[j]);
}
}
else if (prevState[0]==1.0) {
//model: fixed
nextState=prevState;
}
else if (prevState[0]==2.0) {
//model: revolute
BasicDemoRev world;
tempWorldState = world.runSimulation(prevStateWOM,action);
nextState.push_back(2.0); //this is saying you end up in the same state
for(size_t j=0;j<tempWorldState.size();j++){
nextState.push_back(tempWorldState[j]);
}
}
else if (prevState[0]==3.0) {
//model: prismatic
BasicDemoPris world;
tempWorldState = world.runSimulation(prevStateWOM,action);
nextState.push_back(3.0); //this is saying you end up in the same state
for(size_t j=0;j<tempWorldState.size();j++){
nextState.push_back(tempWorldState[j]);
}
}
//print out what the simulation returned each time
if (debug_print_){
std::cout << "the simulation returned:" << std::endl;
for(size_t j=0;j<tempWorldState.size();j++){
std::cout << tempWorldState[j] << "," << std::endl;
}
}
}
return nextState;
}
