Beispiel #1
0
bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
{
	bool allowCreation = true;
	m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation);
	
    if (m_data->m_testBlock1)
    {
        if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
        {
			if (allowSharedMemoryInitialization)
			{
				InitSharedMemoryBlock(m_data->m_testBlock1);
				b3Printf("Created and initialized shared memory block");
				m_data->m_isConnected = true;
			} else
			{
				b3Error("Error: please start server before client\n");
				m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
				m_data->m_testBlock1 = 0;
				return false;
			}
        } else
		{
			b3Printf("Connected to existing shared memory, status OK.\n");
			m_data->m_isConnected = true;
		}
    } else
	{
		b3Error("Cannot connect to shared memory");
		return false;
	}
	return true;
}
Beispiel #2
0
bool PhysicsClientSharedMemory::connect()
{
    ///server always has to create and initialize shared memory
    bool allowCreation = false;
    m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation);

    if (m_data->m_testBlock1)
    {
        if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
        {
            b3Error("Error: please start server before client\n");
            m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
            m_data->m_testBlock1 = 0;
            return false;
        } else
        {
            if (m_data->m_verboseOutput)
            {
                b3Printf("Connected to existing shared memory, status OK.\n");
            }
            m_data->m_isConnected = true;
        }
    } else
    {
        b3Error("Cannot connect to shared memory");
        return false;
    }
    return true;
}
SimpleOpenGL2App::SimpleOpenGL2App(const char* title, int width, int height)
{
	gApp2 = this;
	m_data = new SimpleOpenGL2AppInternalData;

	m_window = new b3gDefaultOpenGLWindow();
	b3gWindowConstructionInfo ci;
	ci.m_title = title;
	ci.m_openglVersion = 2;
	ci.m_width = width;
	ci.m_height = height;
	m_window->createWindow(ci);

	m_window->setWindowTitle(title);

#ifndef __APPLE__
#ifndef _WIN32
//some Linux implementations need the 'glewExperimental' to be true
    glewExperimental = GL_TRUE;
#endif


    if (glewInit() != GLEW_OK)
	{
        b3Error("glewInit failed");
		exit(1);
	}
    if (!GLEW_VERSION_2_1)  // check that the machine supports the 2.1 API.
	{
		b3Error("GLEW_VERSION_2_1 needs to support 2_1");
		exit(1); // or handle the error in a nicer way
	}

#endif
    glGetError();//don't remove this call, it is needed for Ubuntu
	glClearColor(0.9,0.9,1,1);

    b3Assert(glGetError() ==GL_NO_ERROR);

	//m_primRenderer = new GLPrimitiveRenderer(width,height);
	m_parameterInterface = 0;

    b3Assert(glGetError() ==GL_NO_ERROR);

	//m_instancingRenderer = new GLInstancingRenderer(128*1024,32*1024*1024);
	//m_instancingRenderer->init();
	//m_instancingRenderer->resize(width,height);

	b3Assert(glGetError() ==GL_NO_ERROR);

	//m_instancingRenderer->InitShaders();

	m_window->setMouseMoveCallback(Simple2MouseMoveCallback);
	m_window->setMouseButtonCallback(Simple2MouseButtonCallback);
    m_window->setKeyboardCallback(Simple2KeyboardCallback);
    m_window->setWheelCallback(Simple2WheelCallback);
	m_window->setResizeCallback(Simple2ResizeCallback);

}
Beispiel #4
0
bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper)
{
	
	m_data->m_commandProcessor->setGuiHelper(guiHelper);

	
	bool allowCreation = true;
	

    if (m_data->m_isConnected)
    {
        b3Warning("connectSharedMemory, while already connected");
        return m_data->m_isConnected;
    }
    
    
	int counter = 0;
	do 
	{

		m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation);
		if (m_data->m_testBlock1)
		{
			int magicId =m_data->m_testBlock1->m_magicId;
			if (m_data->m_verboseOutput)
			{
				b3Printf("magicId = %d\n", magicId);
			}
        
			if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
			{
				InitSharedMemoryBlock(m_data->m_testBlock1);
				if (m_data->m_verboseOutput)
				{
					b3Printf("Created and initialized shared memory block\n");
				}
				m_data->m_isConnected = true;
			} else
			{
				m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
				m_data->m_testBlock1 = 0;
				m_data->m_isConnected = false;
			}
		} else
		{
			b3Error("Cannot connect to shared memory");
			m_data->m_isConnected = false;
		}
	} while (counter++ < 10 && !m_data->m_isConnected);

	if (!m_data->m_isConnected)
	{
		b3Error("Server cannot connect to shared memory.\n");
	}
	
	return m_data->m_isConnected;
}
void*   PosixSharedMemory::allocateSharedMemory(int key, int size,  bool allowCreation)
{
#ifdef TEST_SHARED_MEMORY
    
    {
        btSharedMemorySegment* seg = 0;
        int i=0;
        
        for (i=0;i<m_internalData->m_segments.size();i++)
        {
            if (m_internalData->m_segments[i].m_key == key)
            {
                seg = &m_internalData->m_segments[i];
                break;
            }
        }
        if (seg)
        {
            b3Error("already created shared memory segment using same key");
            return seg->m_sharedMemoryPtr;
        }

    }
    
    int flags = (allowCreation ? IPC_CREAT : 0) | 0666;
    int id = shmget((key_t) key, (size_t) size,flags);
    if (id < 0)
    {
        b3Error("shmget error");
    } else
    {
        btPointerCaster result;
        result.ptr = shmat(id,0,0);
        if (result.integer == -1)
        {
            b3Error("shmat returned -1");
        } else
        {
            btSharedMemorySegment seg;
            seg.m_key = key;
            seg.m_createdSharedMemory = allowCreation;
            seg.m_sharedMemoryId = id;
            seg.m_sharedMemoryPtr = result.ptr;
            m_internalData->m_segments.push_back(seg);
            return result.ptr;
        }
    }
#else
    //not implemented yet
    btAssert(0);
#endif
    return 0;
}
void PosixSharedMemory::releaseSharedMemory(int key, int size)
{
#ifdef TEST_SHARED_MEMORY
    
    btSharedMemorySegment* seg = 0;
    int i=0;
    
    for (i=0;i<m_internalData->m_segments.size();i++)
    {
        if (m_internalData->m_segments[i].m_key == key)
        {
            seg = &m_internalData->m_segments[i];
            break;
        }
    }

    if (0==seg)
    {
        b3Error("PosixSharedMemory::releaseSharedMemory: shared memory key not found");
        return;
    }
    
    if (seg->m_sharedMemoryId < 0)
    {
        b3Error("PosixSharedMemory::releaseSharedMemory: shared memory id is not set");
    } else
    {
		if (seg->m_createdSharedMemory)
		{
			int result = shmctl(seg->m_sharedMemoryId,IPC_RMID,0);
			if (result == -1)
			{
				b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1");
			} else
			{
				b3Printf("PosixSharedMemory::releaseSharedMemory removed shared memory");
			}
			seg->m_createdSharedMemory = false;
			seg->m_sharedMemoryId = -1;
		}
		if (seg->m_sharedMemoryPtr)
		{
			shmdt(seg->m_sharedMemoryPtr);
			seg->m_sharedMemoryPtr  = 0;
			b3Printf("PosixSharedMemory::releaseSharedMemory detached shared memory\n");
		}
    }

    m_internalData->m_segments.removeAtIndex(i);
    
#endif
}
void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
	PhysicsClient* cl = (PhysicsClient*) userPtr;
	switch (buttonId)
	{
	case  CMD_LOAD_URDF:
	{
		cl->submitCommand(CMD_LOAD_URDF);
		break;
	}
	case CMD_REQUEST_ACTUAL_STATE:
		{
			cl->submitCommand(CMD_REQUEST_ACTUAL_STATE);
			break;
		}
	case CMD_STEP_FORWARD_SIMULATION:
		{
			cl->submitCommand(CMD_STEP_FORWARD_SIMULATION);
			break;
		}
	case CMD_SHUTDOWN:
		{
			cl->submitCommand(CMD_SHUTDOWN);
			break;
		}

	default:
		{
			b3Error("Unknown buttonId");
			btAssert(0);
		}
	};
}
void	PhysicsServer::initPhysics()
{
    createEmptyDynamicsWorld();
    
    m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
    
//    btAssert(m_testBlock1);
    if (m_testBlock1)
    {
      //  btAssert(m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER);
        if (m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER)
        {
            b3Printf("Warning: shared memory is already initialized, did you already spawn a server?\n");
        }
        
        m_testBlock1->m_numClientCommands = 0;
        m_testBlock1->m_numServerCommands = 0;
        m_testBlock1->m_numProcessedClientCommands=0;
        m_testBlock1->m_numProcessedServerCommands=0;
        
        m_testBlock1->m_magicId = SHARED_MEMORY_MAGIC_NUMBER;
        b3Printf("Shared memory succesfully allocated\n");
    } else
    {
        b3Error("Couldn't allocated shared memory, is it implemented on your operating system?\n");
    }
}
void Win32SharedMemory::releaseSharedMemory(int key, int size)
{

    Win32SharedMemorySegment* seg = 0;
    int i=0;
        
    for (i=0;i<m_internalData->m_segments.size();i++)
    {
        if (m_internalData->m_segments[i].m_key == key)
        {
            seg = &m_internalData->m_segments[i];
            break;
        }
    }

	if (seg==0)
	{
		b3Error("Couldn't find shared memory segment");
		return;
	}

	if (seg->m_buf)
	{
		UnmapViewOfFile(seg->m_buf);
		seg->m_buf=0;
	}

	if (seg->m_hMapFile)
	{
		CloseHandle(seg->m_hMapFile);
		seg->m_hMapFile = 0;
	}

	m_internalData->m_segments.removeAtIndex(i);
}
Beispiel #10
0
void LuaPhysicsSetup::initPhysics()
{
	m_guiHelper->setUpAxis(upaxis);
	const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
	int numPrefixes = sizeof(prefix)/sizeof(const char*);
	char relativeFileName[1024];
	FILE* f=0;
	int result = 0;

	for (int i=0;!f && i<numPrefixes;i++)
	{
		sprintf(relativeFileName,"%s%s",prefix[i],sLuaFileName);
		f = fopen(relativeFileName,"rb");
	}
	if (f)
	{
		fclose(f);

		lua_State *L = luaL_newstate();

		luaopen_io(L); // provides io.*
		luaopen_base(L);
		luaopen_table(L);
		luaopen_string(L);
		luaopen_math(L);
		//luaopen_package(L);
		luaL_openlibs(L);

		 // make my_function() available to Lua programs
		lua_register(L, "createDefaultDynamicsWorld", gCreateDefaultDynamicsWorld);
		lua_register(L, "deleteDynamicsWorld", gDeleteDynamicsWorld);
		lua_register(L, "createCubeShape", gCreateCubeShape);
		lua_register(L, "createSphereShape", gCreateSphereShape);
		lua_register(L, "loadMultiBodyFromUrdf",gLoadMultiBodyFromUrdf);

		lua_register(L, "createRigidBody", gCreateRigidBody);
		lua_register(L, "setBodyPosition", gSetBodyPosition);
		lua_register(L, "setBodyOrientation", gSetBodyOrientation);



		int s = luaL_loadfile(L, relativeFileName);

		if ( s==0 ) {
		  // execute Lua program
		  s = lua_pcall(L, 0, LUA_MULTRET, 0);
		}

		report_errors(L, s);
		lua_close(L);
	} else
	{
		b3Error("Cannot find Lua file%s\n",sLuaFileName);
	}

}
bool SharedMemoryCommandProcessor::connect()
{
	if (m_data->m_isConnected)
		return true;

	bool allowCreation = false;
	m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(
		m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation);

	if (m_data->m_testBlock1)
	{
		if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER)
		{
			if ((m_data->m_testBlock1->m_magicId < 211705023) &&
				(m_data->m_testBlock1->m_magicId >= 201705023))
			{
				b3Error("Error: physics server version mismatch (expected %d got %d)\n", SHARED_MEMORY_MAGIC_NUMBER, m_data->m_testBlock1->m_magicId);
			}
			else
			{
				b3Error("Error connecting to shared memory: please start server before client\n");
			}
			m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey,
														SHARED_MEMORY_SIZE);
			m_data->m_testBlock1 = 0;
			return false;
		}
		else
		{
			if (m_data->m_verboseOutput)
			{
				b3Printf("Connected to existing shared memory, status OK.\n");
			}
			m_data->m_isConnected = true;
		}
	}
	else
	{
		b3Error("Cannot connect to shared memory");
		return false;
	}
	return true;
}
Beispiel #12
0
bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper)
{
	m_data->m_guiHelper = guiHelper;
	
	bool allowCreation = true;
	bool allowConnectToExistingSharedMemory = false;

    if (m_data->m_isConnected)
    {
        b3Warning("connectSharedMemory, while already connected");
        return m_data->m_isConnected;
    }
    
    
	m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation);
    if (m_data->m_testBlock1)
    {
        int magicId =m_data->m_testBlock1->m_magicId;
        b3Printf("magicId = %d\n", magicId);
        
        if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
        {
            InitSharedMemoryBlock(m_data->m_testBlock1);
            b3Printf("Created and initialized shared memory block\n");
            m_data->m_isConnected = true;
        } else
		{
			b3Error("Server cannot connect to existing shared memory, disconnecting shared memory.\n");
            m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
            m_data->m_testBlock1 = 0;
            m_data->m_isConnected = false;
		}
    } else
	{
		b3Error("Cannot connect to shared memory");
		m_data->m_isConnected = false;
	}
	return m_data->m_isConnected;
}
bool PhysicsClientSharedMemory::connect() {
    /// server always has to create and initialize shared memory
    bool allowCreation = false;
    m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(
        m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation);

    if (m_data->m_testBlock1) {
        if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) {
            b3Error("Error: please start server before client\n");
            m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey,
                                                        SHARED_MEMORY_SIZE);
            m_data->m_testBlock1 = 0;
            return false;
        } else {
            if (m_data->m_verboseOutput) {
                b3Printf("Connected to existing shared memory, status OK.\n");
            }
            m_data->m_isConnected = true;
        }
    } else {
        b3Warning("Cannot connect to shared memory");
        return false;
    }
#if 0
	if (m_data->m_isConnected)
	{
		//get all existing bodies and body info...

		SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
		//now transfer the information of the individual objects etc.
		command.m_type = CMD_REQUEST_BODY_INFO;
		command.m_sdfRequestInfoArgs.m_bodyUniqueId = 37;
		submitClientCommand(command);
		int timeout = 1024 * 1024 * 1024;
		
		const SharedMemoryStatus* status = 0;

		while ((status == 0) && (timeout-- > 0))
		{
			status = processServerStatus();
		
		}


		//submitClientCommand(command);


	}
#endif
    return true;
}
int b3CpuNarrowPhase::allocateCollidable()
{
	int curSize = m_data->m_collidablesCPU.size();
	if (curSize < m_data->m_config.m_maxConvexShapes)
	{
		m_data->m_collidablesCPU.expand();
		return curSize;
	}
	else
	{
		b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes);
	}
	return -1;
}
Beispiel #15
0
void	PhysicsClient::initPhysics()
{
	if (m_guiHelper && m_guiHelper->getParameterInterface())
	{
		bool isTrigger = false;
		
		createButton("Load URDF",CMD_LOAD_URDF,  isTrigger);
		createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION,  isTrigger);
		createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM,  isTrigger);
		createButton("Get State",CMD_REQUEST_ACTUAL_STATE,  isTrigger);
		createButton("Send Desired State",CMD_SEND_DESIRED_STATE,  isTrigger);
		createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
		
	} else
	{
		m_userCommandRequests.push_back(CMD_LOAD_URDF);
		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
		//m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
		//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
		m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
		//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
		m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
		m_userCommandRequests.push_back(CMD_SHUTDOWN);
		
	}


    m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
    if (m_testBlock1)
    {
     //   btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
        if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
        {
            b3Error("Error: please start server before client\n");
            m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
            m_testBlock1 = 0;
        } else
		{
			b3Printf("Shared Memory status is OK\n");
		}
    } else
	{
		m_wantsTermination = true;
	}
}
int		b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
{
	b3RigidBodyData body;
	int bodyIndex = m_data->m_rigidBodies.size();
	body.m_invMass = mass ? 1.f/mass : 0.f;
	body.m_angVel.setValue(0,0,0);
	body.m_collidableIdx = collidableIndex;
	body.m_frictionCoeff = 0.3f;
	body.m_linVel.setValue(0,0,0);
	body.m_pos.setValue(position[0],position[1],position[2]);
	body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
	body.m_restituitionCoeff = 0.f;

	m_data->m_rigidBodies.push_back(body);

	
	if (collidableIndex>=0)
	{
		b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();

		b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
		b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
		b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
		
		b3Scalar margin = 0.01f;
		b3Transform t;
		t.setIdentity();
		t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
		t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
		b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec);

		m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1);
//		b3Vector3 aabbMin,aabbMax;
	//	m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);

	} else
	{
		b3Error("registerPhysicsInstance using invalid collidableIndex\n");
	}

	return bodyIndex;
}
Beispiel #17
0
int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
{
	b3Assert(shapeIndex == (m_graphicsInstances.size()-1));
	b3Assert(m_graphicsInstances.size()<m_data->m_maxNumObjectCapacity-1);

	b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];

	int index = gfxObj->m_numGraphicsInstances + gfxObj->m_instanceOffset;

	int maxElements = m_data->m_instance_positions_ptr.size();
	if (index*4<maxElements)
	{
		m_data->m_instance_positions_ptr[index*4]=position[0];
		m_data->m_instance_positions_ptr[index*4+1]=position[1];
		m_data->m_instance_positions_ptr[index*4+2]=position[2];
		m_data->m_instance_positions_ptr[index*4+3]=1;

		m_data->m_instance_quaternion_ptr[index*4]=quaternion[0];
		m_data->m_instance_quaternion_ptr[index*4+1]=quaternion[1];
		m_data->m_instance_quaternion_ptr[index*4+2]=quaternion[2];
		m_data->m_instance_quaternion_ptr[index*4+3]=quaternion[3];

		m_data->m_instance_colors_ptr[index*4]=color[0];
		m_data->m_instance_colors_ptr[index*4+1]=color[1];
		m_data->m_instance_colors_ptr[index*4+2]=color[2];
		m_data->m_instance_colors_ptr[index*4+3]=color[3];

		m_data->m_instance_scale_ptr[index*3] = scaling[0];
		m_data->m_instance_scale_ptr[index*3+1] = scaling[1];
		m_data->m_instance_scale_ptr[index*3+2] = scaling[2];

		gfxObj->m_numGraphicsInstances++;
		m_data->m_totalNumInstances++;
	} else
	{
		b3Error("registerGraphicsInstance out of range, %d\n", maxElements);
		return -1;
	}
	return index;//gfxObj->m_numGraphicsInstances;
}
Beispiel #18
0
void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
	PhysicsClient* cl = (PhysicsClient*) userPtr;
	switch (buttonId)
	{
	case  CMD_LOAD_URDF:
	case CMD_CREATE_BOX_COLLISION_SHAPE:
	case CMD_REQUEST_ACTUAL_STATE:
	case CMD_STEP_FORWARD_SIMULATION:
	case CMD_SHUTDOWN:
	case CMD_SEND_BULLET_DATA_STREAM:
		{
			cl->submitCommand(buttonId);
			break;
		}

	default:
		{
			b3Error("Unknown buttonId");
			btAssert(0);
		}
	};
}
bool PhysicsServerSharedMemory::connectSharedMemory(struct GUIHelperInterface* guiHelper)
{
	m_data->m_commandProcessor->setGuiHelper(guiHelper);

	bool allowCreation = true;
	bool allConnected = false;
	int numConnected = 0;

	int counter = 0;
	for (int block = 0; block < MAX_SHARED_MEMORY_BLOCKS; block++)
	{
		if (m_data->m_areConnected[block])
		{
			allConnected = true;
			numConnected++;
			b3Warning("connectSharedMemory, while already connected");
			continue;
		}
		do
		{
			m_data->m_testBlocks[block] = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey + block, SHARED_MEMORY_SIZE, allowCreation);
			if (m_data->m_testBlocks[block])
			{
				int magicId = m_data->m_testBlocks[block]->m_magicId;
				if (m_data->m_verboseOutput)
				{
					b3Printf("magicId = %d\n", magicId);
				}

				if (m_data->m_testBlocks[block]->m_magicId != SHARED_MEMORY_MAGIC_NUMBER)
				{
					InitSharedMemoryBlock(m_data->m_testBlocks[block]);
					if (m_data->m_verboseOutput)
					{
						b3Printf("Created and initialized shared memory block\n");
					}
					m_data->m_areConnected[block] = true;
					numConnected++;
				}
				else
				{
					m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey + block, SHARED_MEMORY_SIZE);
					m_data->m_testBlocks[block] = 0;
					m_data->m_areConnected[block] = false;
				}
			}
			else
			{
				//b3Error("Cannot connect to shared memory");
				m_data->m_areConnected[block] = false;
			}
		} while (counter++ < 10 && !m_data->m_areConnected[block]);
		if (!m_data->m_areConnected[block])
		{
			b3Error("Server cannot connect to shared memory.\n");
		}
	}

	allConnected = (numConnected == MAX_SHARED_MEMORY_BLOCKS);

	return allConnected;
}
Beispiel #20
0
void GLInstancingRenderer::writeTransforms()
{

	b3Assert(glGetError() ==GL_NO_ERROR);


	glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
	glFlush();

	b3Assert(glGetError() ==GL_NO_ERROR);


	char* orgBase =  (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
	if (orgBase)
	{


		int totalNumInstances= 0;

		for (int k=0;k<m_graphicsInstances.size();k++)
		{
			b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
			totalNumInstances+=gfxObj->m_numGraphicsInstances;
		}

		m_data->m_totalNumInstances = totalNumInstances;

		for (int k=0;k<m_graphicsInstances.size();k++)
		{
			//int k=0;
			b3GraphicsInstance* gfxObj = m_graphicsInstances[k];



			int POSITION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
			int ORIENTATION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
			int COLOR_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
		//	int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3);

			char* base = orgBase;

			float* positions = (float*)(base+m_data->m_maxShapeCapacityInBytes);
			float* orientations = (float*)(base+m_data->m_maxShapeCapacityInBytes + POSITION_BUFFER_SIZE);
			float* colors= (float*)(base+m_data->m_maxShapeCapacityInBytes + POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE);
			float* scaling= (float*)(base+m_data->m_maxShapeCapacityInBytes + POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE);

			//static int offset=0;
			//offset++;


			for (int i=0;i<gfxObj->m_numGraphicsInstances;i++)
			{

				int srcIndex=i+gfxObj->m_instanceOffset;

				positions[srcIndex*4] = m_data->m_instance_positions_ptr[srcIndex*4];
				positions[srcIndex*4+1] = m_data->m_instance_positions_ptr[srcIndex*4+1];
				positions[srcIndex*4+2] = m_data->m_instance_positions_ptr[srcIndex*4+2];
				positions[srcIndex*4+3] = m_data->m_instance_positions_ptr[srcIndex*4+3];

				orientations[srcIndex*4]=m_data->m_instance_quaternion_ptr[srcIndex*4];
				orientations[srcIndex*4+1]=m_data->m_instance_quaternion_ptr[srcIndex*4+1];
				orientations[srcIndex*4+2]=m_data->m_instance_quaternion_ptr[srcIndex*4+2];
				orientations[srcIndex*4+3]=m_data->m_instance_quaternion_ptr[srcIndex*4+3];

				colors[srcIndex*4]=m_data->m_instance_colors_ptr[srcIndex*4];
				colors[srcIndex*4+1]=m_data->m_instance_colors_ptr[srcIndex*4+1];
				colors[srcIndex*4+2]=m_data->m_instance_colors_ptr[srcIndex*4+2];
				colors[srcIndex*4+3]=m_data->m_instance_colors_ptr[srcIndex*4+3];

				scaling[srcIndex*3]=m_data->m_instance_scale_ptr[srcIndex*3];
				scaling[srcIndex*3+1]=m_data->m_instance_scale_ptr[srcIndex*3+1];
				scaling[srcIndex*3+2]=m_data->m_instance_scale_ptr[srcIndex*3+2];

			}
		}
	} else
	{
		b3Error("ERROR glMapBuffer failed\n");
	}
	b3Assert(glGetError() ==GL_NO_ERROR);

	glUnmapBuffer( GL_ARRAY_BUFFER);
	//if this glFinish is removed, the animation is not always working/blocks
	//@todo: figure out why
	glFlush();
	glBindBuffer(GL_ARRAY_BUFFER, 0);//m_data->m_vbo);

    b3Assert(glGetError() ==GL_NO_ERROR);

}
Beispiel #21
0
bool	PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus)
{
    bool hasStatus = false;

    if (!m_data->m_testBlock1)
    {
        serverStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
        return true;
    }

    if (!m_data->m_waitingForServer)
    {
        serverStatus.m_type = CMD_WAITING_FOR_CLIENT_COMMAND;
        return true;
    }

    if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands)
    {
        btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1);

        const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
        hasStatus = true;
        serverStatus = serverCmd;
        EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
        //consume the command
        switch (serverCmd.m_type)
        {
        case CMD_CLIENT_COMMAND_COMPLETED:
        {
            if (m_data->m_verboseOutput)
            {
                b3Printf("Server completed command");
            }
            break;
        }
        case CMD_URDF_LOADING_COMPLETED:
        {
            m_data->m_serverLoadUrdfOK = true;
            if (m_data->m_verboseOutput)
            {
                b3Printf("Server loading the URDF OK\n");
            }

            if (serverCmd.m_dataStreamArguments.m_streamChunkLength>0)
            {
                bParse::btBulletFile* bf = new bParse::btBulletFile(this->m_data->m_testBlock1->m_bulletStreamDataServerToClient,serverCmd.m_dataStreamArguments.m_streamChunkLength);
                bf->setFileDNAisMemoryDNA();
                bf->parse(false);
                m_data->m_robotMultiBodyData.push_back(bf);

                for (int i=0; i<bf->m_multiBodies.size(); i++)
                {
                    int flag = bf->getFlags();
                    int qOffset = 7;
                    int uOffset=6;

                    if ((flag&bParse::FD_DOUBLE_PRECISION)!=0)
                    {
                        Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i];
                        if (mb->m_baseName)
                        {
                            if (m_data->m_verboseOutput)
                            {
                                b3Printf("mb->m_baseName = %s\n",mb->m_baseName);
                            }
                        }

                        for (int link=0; link<mb->m_numLinks; link++)
                        {
                            {
                                b3JointInfo info;
                                info.m_flags = 0;
                                info.m_qIndex = qOffset;
                                info.m_uIndex = uOffset;
                                info.m_linkIndex = link;

                                if (mb->m_links[link].m_linkName)
                                {
                                    if (m_data->m_verboseOutput)
                                    {
                                        b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
                                    }
                                    info.m_linkName = mb->m_links[link].m_linkName;
                                }
                                if (mb->m_links[link].m_jointName)
                                {
                                    if (m_data->m_verboseOutput)
                                    {
                                        b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
                                    }
                                    info.m_jointName = mb->m_links[link].m_jointName;
                                    info.m_jointType = mb->m_links[link].m_jointType;
                                }
                                if ((mb->m_links[link].m_jointType == eRevoluteType)||
                                        (mb->m_links[link].m_jointType == ePrismaticType))
                                {
                                    info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
                                }
                                m_data->m_jointInfo.push_back(info);
                            }
                            qOffset+= mb->m_links[link].m_posVarCount;
                            uOffset+= mb->m_links[link].m_dofCount;

                        }

                    } else
                    {
                        Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i];
                        if (mb->m_baseName)
                        {
                            if (m_data->m_verboseOutput)
                            {
                                b3Printf("mb->m_baseName = %s\n",mb->m_baseName);
                            }
                        }
                        for (int link=0; link<mb->m_numLinks; link++)
                        {
                            {
                                b3JointInfo info;
                                info.m_flags = 0;
                                info.m_qIndex = qOffset;
                                info.m_uIndex = uOffset;

                                if (mb->m_links[link].m_linkName)
                                {
                                    if (m_data->m_verboseOutput)
                                    {
                                        b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
                                    }
                                    info.m_linkName = mb->m_links[link].m_linkName;
                                }
                                if (mb->m_links[link].m_jointName)
                                {
                                    if (m_data->m_verboseOutput)
                                    {
                                        b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
                                    }
                                    info.m_jointName = mb->m_links[link].m_jointName;
                                    info.m_jointType = mb->m_links[link].m_jointType;
                                }
                                if ((mb->m_links[link].m_jointType == eRevoluteType)||
                                        (mb->m_links[link].m_jointType == ePrismaticType))
                                {
                                    info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
                                }
                                m_data->m_jointInfo.push_back(info);
                            }
                            qOffset+= mb->m_links[link].m_posVarCount;
                            uOffset+= mb->m_links[link].m_dofCount;
                        }

                    }
                }
                if (bf->ok())
                {
                    if (m_data->m_verboseOutput)
                    {
                        b3Printf("Received robot description ok!\n");
                    }
                } else
                {
                    b3Warning("Robot description not received");
                }
            }
            break;
        }
        case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
        {
            if (m_data->m_verboseOutput)
            {
                b3Printf("Server received desired state");
            }
            break;
        }
        case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
        {
            if (m_data->m_verboseOutput)
            {
                b3Printf("Server completed step simulation");
            }
            break;
        }
        case CMD_URDF_LOADING_FAILED:
        {
            if (m_data->m_verboseOutput)
            {
                b3Printf("Server failed loading the URDF...\n");
            }
            m_data->m_serverLoadUrdfOK = false;
            break;
        }

        case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED:
        {
            if (m_data->m_verboseOutput)
            {
                b3Printf("Server received bullet data stream OK\n");
            }




            break;
        }
        case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED:
        {
            if (m_data->m_verboseOutput)
            {
                b3Printf("Server failed receiving bullet data stream\n");
            }

            break;
        }


        case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
        {
            if (m_data->m_verboseOutput)
            {
                b3Printf("Received actual state\n");
            }
            SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0];

            int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
            int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU;
            if (m_data->m_verboseOutput)
            {
                b3Printf("size Q = %d, size U = %d\n", numQ,numU);
            }
            char msg[1024];

            {
                sprintf(msg,"Q=[");

                for (int i=0; i<numQ; i++)
                {
                    if (i<numQ-1)
                    {
                        sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
                    } else
                    {
                        sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
                    }
                }
                sprintf(msg,"%s]",msg);
            }
            if (m_data->m_verboseOutput)
            {
                b3Printf(msg);
            }

            {
                sprintf(msg,"U=[");

                for (int i=0; i<numU; i++)
                {
                    if (i<numU-1)
                    {
                        sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]);
                    } else
                    {
                        sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]);
                    }
                }
                sprintf(msg,"%s]",msg);

            }
            if (m_data->m_verboseOutput)
            {
                b3Printf(msg);
            }


            if (m_data->m_verboseOutput)
            {
                b3Printf("\n");
            }
            break;
        }
        case 	CMD_DEBUG_LINES_COMPLETED:
        {
            if (m_data->m_verboseOutput)
            {
                b3Printf("Success receiving %d debug lines",serverCmd.m_sendDebugLinesArgs.m_numDebugLines);
            }

            int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines;
            btScalar* linesFrom = (btScalar*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0];
            btScalar* linesTo = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*sizeof(btVector3));
            btScalar* linesColor = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*sizeof(btVector3));

            m_data->m_debugLinesFrom.resize(numLines);
            m_data->m_debugLinesTo.resize(numLines);
            m_data->m_debugLinesColor.resize(numLines);

            for (int i=0; i<numLines; i++)
            {
                btVector3 from(linesFrom[i*4],linesFrom[i*4+1],linesFrom[i*4+2]);
                btVector3 to(linesTo[i*4],linesTo[i*4+1],linesTo[i*4+2]);
                btVector3 color(linesColor[i*4],linesColor[i*4+1],linesColor[i*4+2]);

                m_data->m_debugLinesFrom[i] = from;
                m_data->m_debugLinesTo[i] = to;
                m_data->m_debugLinesColor[i] = color;
            }
            break;
        }
        case CMD_DEBUG_LINES_OVERFLOW_FAILED:
        {
            b3Warning("Error receiving debug lines");
            m_data->m_debugLinesFrom.resize(0);
            m_data->m_debugLinesTo.resize(0);
            m_data->m_debugLinesColor.resize(0);

            break;
        }

        default:
        {
            b3Error("Unknown server status\n");
            btAssert(0);
        }
        };


        m_data->m_testBlock1->m_numProcessedServerCommands++;
        //we don't have more than 1 command outstanding (in total, either server or client)
        btAssert(m_data->m_testBlock1->m_numProcessedServerCommands == m_data->m_testBlock1->m_numServerCommands);

        if (m_data->m_testBlock1->m_numServerCommands == m_data->m_testBlock1->m_numProcessedServerCommands)
        {
            m_data->m_waitingForServer = false;
        } else
        {
            m_data->m_waitingForServer = true;
        }
    } else
    {
        if (m_data->m_verboseOutput)
        {
            b3Printf("m_numServerStatus  = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands,
                     m_data->m_testBlock1->m_numProcessedServerCommands);
        }
    }
    return hasStatus;
}
inline int b3GpuPgsContactSolver::sortConstraintByBatch3( b3Contact4* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies, int* batchSizes)
{
	
	B3_PROFILE("sortConstraintByBatch3");
	
	static int maxSwaps = 0;
	int numSwaps = 0;

	curUsed.resize(2*simdWidth);

	static int maxNumConstraints = 0;
	if (maxNumConstraints<numConstraints)
	{
		maxNumConstraints = numConstraints;
		//printf("maxNumConstraints  = %d\n",maxNumConstraints );
	}

	int numUsedArray = numBodies/32+1;
	bodyUsed.resize(numUsedArray);

	for (int q=0;q<numUsedArray;q++)
		bodyUsed[q]=0;

	
	int curBodyUsed = 0;

	int numIter = 0;
    
	m_data->m_sortData.resize(0);
	m_data->m_idxBuffer.resize(0);
	m_data->m_old.resize(0);
	
		
#if defined(_DEBUG)
	for(int i=0; i<numConstraints; i++)
		cs[i].getBatchIdx() = -1;
#endif
	
	int numValidConstraints = 0;
	int unprocessedConstraintIndex = 0;

	int batchIdx = 0;
    

	{
		B3_PROFILE("cpu batch innerloop");
		
		while( numValidConstraints < numConstraints)
		{
			numIter++;
			int nCurrentBatch = 0;
			batchSizes[batchIdx] = 0;

			//	clear flag
			for(int i=0; i<curBodyUsed; i++) 
				bodyUsed[curUsed[i]/32] = 0;

            curBodyUsed = 0;

			for(int i=numValidConstraints; i<numConstraints; i++)
			{
				int idx = i;
				b3Assert( idx < numConstraints );
				//	check if it can go
				int bodyAS = cs[idx].m_bodyAPtrAndSignBit;
				int bodyBS = cs[idx].m_bodyBPtrAndSignBit;
				int bodyA = abs(bodyAS);
				int bodyB = abs(bodyBS);
				bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx;
				bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx;
				int aUnavailable = 0;
				int bUnavailable = 0;
				if (!aIsStatic)
				{
					aUnavailable = bodyUsed[ bodyA/32 ] & (1<<(bodyA&31));
				}
				if (!aUnavailable)
				if (!bIsStatic)
				{
					bUnavailable = bodyUsed[ bodyB/32 ] & (1<<(bodyB&31));
				}
                
				if( aUnavailable==0 && bUnavailable==0 ) // ok
				{
					if (!aIsStatic)
					{
						bodyUsed[ bodyA/32 ] |= (1<<(bodyA&31));
						curUsed[curBodyUsed++]=bodyA;
					}
					if (!bIsStatic)
					{
						bodyUsed[ bodyB/32 ] |= (1<<(bodyB&31));
						curUsed[curBodyUsed++]=bodyB;
					}

					cs[idx].getBatchIdx() = batchIdx;

					if (i!=numValidConstraints)
					{
						b3Swap(cs[i],cs[numValidConstraints]);
						numSwaps++;
					}

					numValidConstraints++;
					{
						nCurrentBatch++;
						if( nCurrentBatch == simdWidth )
						{
							batchSizes[batchIdx] += simdWidth;
							nCurrentBatch = 0;
							for(int i=0; i<curBodyUsed; i++) 
								bodyUsed[curUsed[i]/32] = 0;
							curBodyUsed = 0;
						}
					}
				}
			}

			if (batchIdx>=B3_MAX_NUM_BATCHES)
			{
				b3Error("batchIdx>=B3_MAX_NUM_BATCHES");
				b3Assert(0);
				break;
			}

			batchSizes[batchIdx] += nCurrentBatch;

			batchIdx ++;
			
		}
	}
	
#if defined(_DEBUG)
    //		debugPrintf( "nBatches: %d\n", batchIdx );
	for(int i=0; i<numConstraints; i++)
    {
        b3Assert( cs[i].getBatchIdx() != -1 );
    }
#endif

	batchSizes[batchIdx] =0;
	
	if (maxSwaps<numSwaps)
	{
		maxSwaps = numSwaps;
		//printf("maxSwaps = %d\n", maxSwaps);
	}
	
	return batchIdx;
}
Beispiel #23
0
	virtual void reportError(const char* error)
	{
		m_numErrors++;
		b3Error(error);
	}
Beispiel #24
0
void	b3GpuDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies)
{
	constraint->setUserConstraintPtr(0);

	m_constraints.push_back(constraint);
	
	switch (constraint->getConstraintType())
	{
	case POINT2POINT_CONSTRAINT_TYPE:
		{
			btPoint2PointConstraint* p = (btPoint2PointConstraint*) constraint;
			int rbA = p->getRigidBodyA().getUserIndex();
			int rbB = p->getRigidBodyB().getUserIndex();
			btVector3 pivotInB = p->getPivotInB();
			if (rbB<=0)
			{
				pivotInB = p->getRigidBodyA().getWorldTransform()*p->getPivotInA();
				rbB = m_staticBody->getUserIndex();
			}
			if (rbA>=0 && rbB>=0)
			{
				b3Point2PointConstraint* p2p = new b3Point2PointConstraint(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
				p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold());
				constraint->setUserConstraintPtr(p2p);
				m_rigidBodyPipeline->addConstraint(p2p);
			} else
			{
				b3Error("invalid body index in addConstraint,b3Point2PointConstraint\n");
			}
			break;
		}
	case D6_CONSTRAINT_TYPE:
		{
			btGeneric6DofConstraint* dof2  = (btGeneric6DofConstraint*) constraint;
			const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu();

			int rbA = dof2->getRigidBodyA().getUserIndex();
			int rbB = dof2->getRigidBodyB().getUserIndex();

			btTransform frameInA = dof2->getFrameOffsetB();
			btTransform frameInB = dof2->getFrameOffsetB();

			if (rbA<=0)
			{
				frameInA = dof2->getRigidBodyB().getWorldTransform()*dof2->getFrameOffsetB();
				rbA = m_staticBody->getUserIndex();
			}

			if (rbB<=0)
			{
				frameInB = dof2->getRigidBodyA().getWorldTransform()*dof2->getFrameOffsetA();
				rbB = m_staticBody->getUserIndex();
			}
			if (rbA>=0 && rbB>=0)
			{
				b3Generic6DofConstraint* dof3 = new b3Generic6DofConstraint(rbA,rbB,(b3Transform&)frameInA,(b3Transform&)frameInB,false,bodiesCL);//(();//(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
				{
					btVector3 limit(0,0,0);
					dof2->getLinearLowerLimit(limit);
					dof3->setLinearLowerLimit((const b3Vector3&)limit);

					dof2->setLinearUpperLimit(limit);
					dof3->setLinearUpperLimit((const b3Vector3&)limit);

					dof2->setAngularLowerLimit(limit);
					dof3->setAngularLowerLimit((const b3Vector3&)limit);

					dof2->setAngularUpperLimit(limit);
					dof3->setAngularUpperLimit((const b3Vector3&)limit);
				/*	for (int i=0;i<6;i++)
					{
						dof3->setParam(BT_CONSTRAINT_STOP_CFM,dof2->getParam(BT_CONSTRAINT_STOP_CFM,i),i);
						dof3->setParam(BT_CONSTRAINT_STOP_ERP,dof2->getParam(BT_CONSTRAINT_STOP_ERP,i),i);
					}
					*/
					dof3->setBreakingImpulseThreshold(dof2->getBreakingImpulseThreshold());
				}
//				p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold());
				constraint->setUserConstraintPtr(dof3);
				m_rigidBodyPipeline->addConstraint(dof3);
			} else
			{
				b3Error("invalid body index in addConstraint, btGeneric6DofConstraint.\n");
			}

//			b3Generic6DofConstraint
			break;
		}
	default:
		b3Warning("Warning: b3GpuDynamicsWorld::addConstraint with unsupported constraint type\n");
	};


}
Beispiel #25
0
	virtual void reportError(const char* error)
	{
		b3Error(error);
	}
void	PhysicsServer::stepSimulation(float deltaTime)
{

    bool wantsShutdown = false;
    
    if (m_testBlock1)
    {
        ///we ignore overflow of integer for now
        if (m_testBlock1->m_numClientCommands> m_testBlock1->m_numProcessedClientCommands)
        {
            
            //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
            btAssert(m_testBlock1->m_numClientCommands==m_testBlock1->m_numProcessedClientCommands+1);
            
            const SharedMemoryCommand& clientCmd =m_testBlock1->m_clientCommands[0];
             m_testBlock1->m_numProcessedClientCommands++;
            
            //consume the command
            switch (clientCmd.m_type)
            {
                case CMD_LOAD_URDF:
                {
                    b3Printf("Processed CMD_LOAD_URDF:%s",clientCmd.m_urdfArguments.m_urdfFileName);
                    
                    //load the actual URDF and send a report: completed or failed

                    
                    bool completedOk = loadUrdf(clientCmd.m_urdfArguments.m_urdfFileName,
                                               btVector3(0,0,0), btQuaternion(0,0,0,1),true,true );
                    SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
 
                    if (completedOk)
                    {
                        serverCmd.m_type =CMD_URDF_LOADING_COMPLETED;
                    } else
                    {
                        serverCmd.m_type =CMD_URDF_LOADING_FAILED;
                    
                    }
                    m_testBlock1->m_numServerCommands++;
                    break;
                }
                case CMD_STEP_FORWARD_SIMULATION:
                {
                   
                    b3Printf("Step simulation request");
                    double timeStep = clientCmd.m_stepSimulationArguments.m_deltaTimeInSeconds;
                    m_dynamicsWorld->stepSimulation(timeStep);
                    
                    SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
                    
                    serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED;
                    m_testBlock1->m_numServerCommands++;
 
                    break;
                }
                case CMD_SHUTDOWN:
                {
                    wantsShutdown = true;
                    break;
                }
                default:
                {
                    b3Error("Unsupported command encountered");
                    btAssert(0);
                }
            };
            
           
            
            //process the command right now
            
        }
    }
    if (wantsShutdown)
    {
        m_wantsShutdown = true;
        releaseSharedMemory();
    }
}
void  b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
{
	B3_PROFILE("b3GpuGridBroadphase::calculateOverlappingPairs");
	

	if (0)
	{
		calculateOverlappingPairsHost(maxPairs);
	/*
		b3AlignedObjectArray<b3Int4> cpuPairs;
		m_gpuPairs.copyToHost(cpuPairs);
		printf("host m_gpuPairs.size()=%d\n",m_gpuPairs.size());
		for (int i=0;i<m_gpuPairs.size();i++)
		{
			printf("host pair %d = %d,%d\n",i,cpuPairs[i].x,cpuPairs[i].y);
		}
		*/
		return;
	}
	
	


	
	int numSmallAabbs = m_smallAabbsMappingGPU.size();

	b3OpenCLArray<int> pairCount(m_context,m_queue);
	pairCount.push_back(0);
	m_gpuPairs.resize(maxPairs);//numSmallAabbs*maxPairsPerBody);

	{
		int numLargeAabbs = m_largeAabbsMappingGPU.size();
		if (numLargeAabbs && numSmallAabbs)
		{
			B3_PROFILE("sap2Kernel");
			b3BufferInfoCL bInfo[] = { 
				b3BufferInfoCL( m_allAabbsGPU1.getBufferCL() ),
				b3BufferInfoCL( m_largeAabbsMappingGPU.getBufferCL() ),
				b3BufferInfoCL( m_smallAabbsMappingGPU.getBufferCL() ), 
				b3BufferInfoCL( m_gpuPairs.getBufferCL() ), 
				b3BufferInfoCL(pairCount.getBufferCL())};
			b3LauncherCL launcher(m_queue, m_sap2Kernel,"m_sap2Kernel");
			launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
			launcher.setConst(   numLargeAabbs  );
			launcher.setConst( numSmallAabbs);
			launcher.setConst( 0  );//axis is not used
			launcher.setConst( maxPairs  );
	//@todo: use actual maximum work item sizes of the device instead of hardcoded values
			launcher.launch2D( numLargeAabbs, numSmallAabbs,4,64);
                
			int numPairs = pairCount.at(0);
			
			if (numPairs >maxPairs)
			{
				b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
				numPairs =maxPairs;
			}
		}
	}




	if (numSmallAabbs)
	{
		B3_PROFILE("gridKernel");
		m_hashGpu.resize(numSmallAabbs);
		{
			B3_PROFILE("kCalcHashAABB");
			b3LauncherCL launch(m_queue,kCalcHashAABB,"kCalcHashAABB");
			launch.setConst(numSmallAabbs);
			launch.setBuffer(m_allAabbsGPU1.getBufferCL());
			launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
			launch.setBuffer(m_hashGpu.getBufferCL());
			launch.setBuffer(this->m_paramsGPU.getBufferCL());
			launch.launch1D(numSmallAabbs);
		}

		m_sorter->execute(m_hashGpu);
		
		int numCells = this->m_paramsCPU.m_gridSize[0]*this->m_paramsCPU.m_gridSize[1]*this->m_paramsCPU.m_gridSize[2];
		m_cellStartGpu.resize(numCells);
		//b3AlignedObjectArray<int >			cellStartCpu;
				
		
		{
			B3_PROFILE("kClearCellStart");
			b3LauncherCL launch(m_queue,kClearCellStart,"kClearCellStart");
			launch.setConst(numCells);
			launch.setBuffer(m_cellStartGpu.getBufferCL());
			launch.launch1D(numCells);
			//m_cellStartGpu.copyToHost(cellStartCpu);
			//printf("??\n");

		}


		{
			B3_PROFILE("kFindCellStart");
			b3LauncherCL launch(m_queue,kFindCellStart,"kFindCellStart");
			launch.setConst(numSmallAabbs);
			launch.setBuffer(m_hashGpu.getBufferCL());
			launch.setBuffer(m_cellStartGpu.getBufferCL());
			launch.launch1D(numSmallAabbs);
			//m_cellStartGpu.copyToHost(cellStartCpu);
			//printf("??\n");

		}
		
		{
			B3_PROFILE("kFindOverlappingPairs");
			
			
			b3LauncherCL launch(m_queue,kFindOverlappingPairs,"kFindOverlappingPairs");
			launch.setConst(numSmallAabbs);
			launch.setBuffer(m_allAabbsGPU1.getBufferCL());
			launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
			launch.setBuffer(m_hashGpu.getBufferCL());
			launch.setBuffer(m_cellStartGpu.getBufferCL());
			
			launch.setBuffer(m_paramsGPU.getBufferCL());
			//launch.setBuffer(0);
			launch.setBuffer(pairCount.getBufferCL());
			launch.setBuffer(m_gpuPairs.getBufferCL());
			
			launch.setConst(maxPairs);
			launch.launch1D(numSmallAabbs);
			

			int numPairs = pairCount.at(0);
			if (numPairs >maxPairs)
			{
				b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
				numPairs =maxPairs;
			}
			
			m_gpuPairs.resize(numPairs);
	
			if (0)
			{
				b3AlignedObjectArray<b3Int4> pairsCpu;
				m_gpuPairs.copyToHost(pairsCpu);
			
				printf("m_gpuPairs.size()=%d\n",m_gpuPairs.size());
				for (int i=0;i<m_gpuPairs.size();i++)
				{
					printf("pair %d = %d,%d\n",i,pairsCpu[i].x,pairsCpu[i].y);
				}

				printf("?!?\n");
			}
			
		}
	

	}

	



	//calculateOverlappingPairsHost(maxPairs);
}
void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
	PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;

	SharedMemoryCommand command;

	switch (buttonId)
	{
	case  CMD_LOAD_URDF:
		{
			command.m_type =CMD_LOAD_URDF;
            sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");//kuka_lwr/kuka.urdf");
            command.m_urdfArguments.m_initialPosition[0] = 0.0;
			command.m_updateFlags = 
				URDF_ARGS_FILE_NAME| URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION|URDF_ARGS_USE_MULTIBODY|URDF_ARGS_USE_FIXED_BASE;

			command.m_urdfArguments.m_initialPosition[1] = 0.0;
			command.m_urdfArguments.m_initialPosition[2] = 0.0;
			command.m_urdfArguments.m_initialOrientation[0] = 0.0;
			command.m_urdfArguments.m_initialOrientation[1] = 0.0;
			command.m_urdfArguments.m_initialOrientation[2] = 0.0;
			command.m_urdfArguments.m_initialOrientation[3] = 1.0;
			command.m_urdfArguments.m_useFixedBase = false;
			command.m_urdfArguments.m_useMultiBody = true;
			cl->enqueueCommand(command);
			break;
		}
	case CMD_CREATE_BOX_COLLISION_SHAPE:
		{
			command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
			command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION;
			command.m_createBoxShapeArguments.m_initialPosition[0] = 0;
			command.m_createBoxShapeArguments.m_initialPosition[1] = 0;
			command.m_createBoxShapeArguments.m_initialPosition[2] = -3;
			
			cl->enqueueCommand(command);
			break;
		}
	case CMD_REQUEST_ACTUAL_STATE:
		{
			command.m_type =CMD_REQUEST_ACTUAL_STATE;
			cl->enqueueCommand(command);
			break;
		};
	case CMD_STEP_FORWARD_SIMULATION:
		{
			command.m_type =CMD_STEP_FORWARD_SIMULATION;
			cl->enqueueCommand(command);
			command.m_type =CMD_REQUEST_DEBUG_LINES;
            command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//:DBG_DrawConstraints;
            command.m_requestDebugLinesArguments.m_startingLineIndex = 0;
			cl->enqueueCommand(command);
			break;
		}
	
    case CMD_SEND_DESIRED_STATE:
		{
			
            command.m_type =CMD_SEND_DESIRED_STATE;
            
			cl->prepareControlCommand(command);

			cl->enqueueCommand(command);		
			break;
		}
		case CMD_RESET_SIMULATION:
		{
			command.m_type = CMD_RESET_SIMULATION;
			cl->enqueueCommand(command);
			break;
		}
	case CMD_SEND_BULLET_DATA_STREAM:
		{
			command.m_type = buttonId;
			cl->enqueueCommand(command);
			break;
		}

	default:
		{
			b3Error("Unknown buttonId");
			btAssert(0);
		}
	};
}
void*   Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
{
	{
		
        Win32SharedMemorySegment* seg = 0;
        int i=0;
        
        for (i=0;i<m_internalData->m_segments.size();i++)
        {
            if (m_internalData->m_segments[i].m_key == key)
            {
                seg = &m_internalData->m_segments[i];
                break;
            }
        }
        if (seg)
        {
            b3Error("already created shared memory segment using same key");
            return seg->m_buf;
        }

    }

	Win32SharedMemorySegment seg;
	seg.m_key = key;
#ifdef UNICODE
	swprintf_s (seg.m_szName,TEXT("MyFileMappingObject%d"),key);
#else
	
	sprintf(seg.m_szName,"MyFileMappingObject%d",key);	
#endif


	seg.m_hMapFile = OpenFileMapping(
                   FILE_MAP_ALL_ACCESS,   // read/write access
                   FALSE,                 // do not inherit the name
                   seg.m_szName);               // name of mapping object

	if (seg.m_hMapFile==NULL)
	{
		 if (allowCreation)
		 {
			  seg.m_hMapFile = CreateFileMapping(
                 INVALID_HANDLE_VALUE,    // use paging file
                 NULL,                    // default security
                 PAGE_READWRITE,          // read/write access
                 0,                       // maximum object size (high-order DWORD)
                 size,						// maximum object size (low-order DWORD)
                 seg.m_szName);                 // name of mapping object
		 } else
		 {
			   b3Warning("Could not create file mapping object (%d).\n",GetLastError());
			 return 0;
		 }

	}

   seg.m_buf =  MapViewOfFile(seg.m_hMapFile,   // handle to map object
                        FILE_MAP_ALL_ACCESS, // read/write permission
                        0,
                        0,
                        size);

   if (seg.m_buf == NULL)
	{
		b3Warning("Could not map view of file (%d).\n",GetLastError());
		CloseHandle(seg.m_hMapFile);
		return 0;
   }

   m_internalData->m_segments.push_back(seg);
   return seg.m_buf;
}
void MyCallback2(int buttonId, bool buttonState, void* userPtr)
{
	RobotControlExample* cl = (RobotControlExample*) userPtr;

	SharedMemoryCommand command;

	switch (buttonId)
	{
	case  CMD_LOAD_URDF:
		{
			command.m_type =CMD_LOAD_URDF;
			command.m_updateFlags = URDF_ARGS_FILE_NAME|URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION;
			sprintf(command.m_urdfArguments.m_urdfFileName,"kuka_lwr/kuka.urdf");//r2d2.urdf");
			command.m_urdfArguments.m_initialPosition[0] = 0.0;
			command.m_urdfArguments.m_initialPosition[1] = 0.0;
			command.m_urdfArguments.m_initialPosition[2] = 0.0;
			command.m_urdfArguments.m_initialOrientation[0] = 0.0;
			command.m_urdfArguments.m_initialOrientation[1] = 0.0;
			command.m_urdfArguments.m_initialOrientation[2] = 0.0;
			command.m_urdfArguments.m_initialOrientation[3] = 1.0;
			command.m_urdfArguments.m_useFixedBase = false;
			command.m_urdfArguments.m_useMultiBody = true;
			cl->enqueueCommand(command);
			break;
		}

		case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
		{
		    //#ifdef USE_C_API
		    b3InitPhysicsParamCommand(&command);
            b3PhysicsParamSetGravity(&command, 1,1,-10);
            

//		    #else
//		    
//			command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS;
//			command.m_physSimParamArgs.m_gravityAcceleration[0] = 0;
//			command.m_physSimParamArgs.m_gravityAcceleration[1] = 0;
//			command.m_physSimParamArgs.m_gravityAcceleration[2] = -10;
//			command.m_physSimParamArgs.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY;
//			#endif // USE_C_API
			
			cl->enqueueCommand(command);
			break;

		};
		case CMD_INIT_POSE:
		{
			///@todo: implement this
			command.m_type = CMD_INIT_POSE;
			cl->enqueueCommand(command);
			break;
		}

			
	case CMD_CREATE_BOX_COLLISION_SHAPE:
		{
			command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
            command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION;
            command.m_createBoxShapeArguments.m_initialPosition[0] = 0;
            command.m_createBoxShapeArguments.m_initialPosition[1] = 0;
            command.m_createBoxShapeArguments.m_initialPosition[2] = -3;
            
            
			cl->enqueueCommand(command);
			break;
		}
	case CMD_REQUEST_ACTUAL_STATE:
		{
			command.m_type =CMD_REQUEST_ACTUAL_STATE;
			cl->enqueueCommand(command);
			break;
		};
	case CMD_STEP_FORWARD_SIMULATION:
		{
			command.m_type =CMD_STEP_FORWARD_SIMULATION;
			cl->enqueueCommand(command);
			break;
		}
	
    case CMD_SEND_DESIRED_STATE:
		{
			
			command.m_type =CMD_SEND_DESIRED_STATE;
			cl->prepareControlCommand(command);
			cl->enqueueCommand(command);		
			break;
		}
	case CMD_SEND_BULLET_DATA_STREAM:
		{
			command.m_type = buttonId;
			sprintf(command.m_dataStreamArguments.m_bulletFileName,"slope.bullet");
			command.m_dataStreamArguments.m_streamChunkLength = 0;
			cl->enqueueCommand(command);
			break;
		}

	default:
		{
			b3Error("Unknown buttonId");
			btAssert(0);
		}
	};
}