Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add pybullet.multiplyTransforms and pybullet.invertTransform

Browse Source 
use btAssert instead of assert (glGetError is really slow)
shift default light-position a little bit, to make faces different color
This commit is contained in:
erwincoumans
2017-05-29 21:55:58 -07:00
parent 5436b8f048
commit 4e03c36fa6
7 changed files with 218 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -225,7 +225,7 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
m_shadowTexture(0),
m_renderFrameBuffer(0)
{
m_lightPos=b3MakeVector3(-50,50,50);
m_lightPos=b3MakeVector3(-50,30,40);
//clear to zero to make it obvious if the matrix is used uninitialized
for (int i=0;i<16;i++)
@@ -405,6 +405,10 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3Assert(pg);
if (pg==0)
return;
int srcIndex = pg->m_internalInstanceIndex;
b3Assert(srcIndex<m_data->m_totalNumInstances);

90
examples/OpenGLWindow/GLPrimitiveRenderer.cpp
View File

@@ -1,10 +1,10 @@
#ifndef NO_OPENGL3
#include "GLPrimitiveRenderer.h"
#include "GLPrimInternalData.h"
//#include "Bullet3Common/b3Logging.h"
#include "Bullet3Common/b3Scalar.h"
#include "LoadShader.h"
#include <assert.h>
static const char* vertexShader3D= \
"#version 150 \n"
@@ -75,27 +75,27 @@ m_screenHeight(screenHeight)
m_data->m_viewmatUniform = glGetUniformLocation(m_data->m_shaderProg,"viewMatrix");
if (m_data->m_viewmatUniform < 0) {
assert(0);
b3Assert(0);
}
m_data->m_projMatUniform = glGetUniformLocation(m_data->m_shaderProg,"projMatrix");
if (m_data->m_projMatUniform < 0) {
assert(0);
b3Assert(0);
}
m_data->m_positionUniform = glGetUniformLocation(m_data->m_shaderProg, "p");
if (m_data->m_positionUniform < 0) {
assert(0);
b3Assert(0);
}
m_data->m_colourAttribute = glGetAttribLocation(m_data->m_shaderProg, "colour");
if (m_data->m_colourAttribute < 0) {
assert(0);
b3Assert(0);
}
m_data->m_positionAttribute = glGetAttribLocation(m_data->m_shaderProg, "position");
if (m_data->m_positionAttribute < 0) {
assert(0);
b3Assert(0);
}
m_data->m_textureAttribute = glGetAttribLocation(m_data->m_shaderProg,"texuv");
if (m_data->m_textureAttribute < 0) {
assert(0);
b3Assert(0);
}
loadBufferData();
@@ -127,7 +127,7 @@ void GLPrimitiveRenderer::loadBufferData()
glBufferData(GL_ARRAY_BUFFER, MAX_VERTICES2 * sizeof(PrimVertex), 0, GL_DYNAMIC_DRAW);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
@@ -153,14 +153,14 @@ void GLPrimitiveRenderer::loadBufferData()
glEnableVertexAttribArray(m_data->m_positionAttribute);
glEnableVertexAttribArray(m_data->m_colourAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_textureAttribute);
glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
@@ -200,7 +200,7 @@ void GLPrimitiveRenderer::loadBufferData()
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 256,256,0,GL_RGB,GL_UNSIGNED_BYTE,image);
glGenerateMipmap(GL_TEXTURE_2D);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
delete[] image;
@@ -226,14 +226,14 @@ void GLPrimitiveRenderer::drawLine()
void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float color[4])
{
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glActiveTexture(GL_TEXTURE0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
drawTexturedRect(x0,y0,x1,y1,color,0,0,1,1);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
}
@@ -242,7 +242,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
{
//B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D");
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glUseProgram(m_data->m_shaderProg);
@@ -250,7 +250,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
glUniformMatrix4fv(m_data->m_viewmatUniform, 1, false, viewMat);
glUniformMatrix4fv(m_data->m_projMatUniform, 1, false, projMat);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer);
glBindVertexArray(m_data->m_vertexArrayObject);
@@ -277,7 +277,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
PrimVec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) );
if (useRGBA)
@@ -290,47 +290,47 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_positionAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_colourAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_textureAttribute);
glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer);
//glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
int indexCount = 6;
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindVertexArray(0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ARRAY_BUFFER, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
//glDisableVertexAttribArray(m_data->m_textureAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glUseProgram(0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
}
@@ -349,7 +349,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVert
}
//B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D");
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
float identity[16]={1,0,0,0,
0,1,0,0,
0,0,1,0,
@@ -360,7 +360,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVert
glUniformMatrix4fv(m_data->m_viewmatUniform, 1, false, identity);
glUniformMatrix4fv(m_data->m_projMatUniform, 1, false, identity);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer2);
glBindVertexArray(m_data->m_vertexArrayObject2);
@@ -388,7 +388,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVert
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
PrimVec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) );
if (useRGBA)
@@ -401,47 +401,47 @@ void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVert
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_positionAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_colourAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_textureAttribute);
glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer2);
//glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
int indexCount = (numVertices/4)*6;
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindVertexArray(0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ARRAY_BUFFER, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
//glDisableVertexAttribArray(m_data->m_textureAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glUseProgram(0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
}
@@ -477,7 +477,7 @@ void GLPrimitiveRenderer::flushBatchedRects()
return;
glActiveTexture(GL_TEXTURE0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle);
drawTexturedRect3D2(m_data2->m_verticesRect, m_data2->m_numVerticesRect,0);
m_data2->m_numVerticesRect=0;

35
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -3159,3 +3159,38 @@ double b3GetTimeOut(b3PhysicsClientHandle physClient)
}
return -1;
}
void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4])
{
b3Transform trA;
b3Transform trB;
trA.setOrigin(b3MakeVector3(posA[0],posA[1],posA[2]));
trA.setRotation(b3Quaternion(ornA[0],ornA[1],ornA[2],ornA[3]));
trB.setOrigin(b3MakeVector3(posB[0],posB[1],posB[2]));
trB.setRotation(b3Quaternion(ornB[0],ornB[1],ornB[2],ornB[3]));
b3Transform res = trA*trB;
outPos[0] = res.getOrigin()[0];
outPos[1] = res.getOrigin()[1];
outPos[2] = res.getOrigin()[2];
b3Quaternion orn = res.getRotation();
outOrn[0] = orn[0];
outOrn[1] = orn[1];
outOrn[2] = orn[2];
outOrn[3] = orn[3];
}
void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4])
{
b3Transform tr;
tr.setOrigin(b3MakeVector3(pos[0],pos[1],pos[2]));
tr.setRotation(b3Quaternion(orn[0],orn[1],orn[2],orn[3]));
b3Transform trInv = tr.inverse();
outPos[0] = trInv.getOrigin()[0];
outPos[1] = trInv.getOrigin()[1];
outPos[2] = trInv.getOrigin()[2];
b3Quaternion invOrn = trInv.getRotation();
outOrn[0] = invOrn[0];
outOrn[1] = invOrn[1];
outOrn[2] = invOrn[2];
outOrn[3] = invOrn[3];
}

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -416,6 +416,8 @@ void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle comma
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
double b3GetTimeOut(b3PhysicsClientHandle physClient);
void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4]);
void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4]);
#ifdef __cplusplus
}

18
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -1813,6 +1813,13 @@ void PhysicsServerExample::updateGraphics()
}
case eGUIHelperRemoveAllGraphicsInstances:
{
#ifdef BT_ENABLE_VR
if (m_tinyVrGui)
{
delete m_tinyVrGui;
m_tinyVrGui=0;
}
#endif //BT_ENABLE_VR
m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
{
@@ -2224,8 +2231,11 @@ void PhysicsServerExample::renderScene()
{
static int frameCount=0;
//static btScalar prevTime = m_clock.getTimeSeconds();
static btScalar prevTime = m_clock.getTimeSeconds();
frameCount++;
static char line0[1024]={0};
static char line1[1024]={0};
#if 0
@@ -2233,7 +2243,8 @@ void PhysicsServerExample::renderScene()
int numFrames = 200;
static int count = 0;
count++;
if (0 == (count & 1))
{
btScalar curTime = m_clock.getTimeSeconds();
@@ -2279,9 +2290,6 @@ void PhysicsServerExample::renderScene()
tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0));
b3Scalar dt = 0.01;
m_tinyVrGui->clearTextArea();
static char line0[1024];
static char line1[1024];
m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255);
m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255);

4
examples/pybullet/examples/restitution.py
View File

@@ -4,7 +4,9 @@
import pybullet as p
import time
p.connect(p.GUI)
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
cid = p.connect(p.GUI)
restitutionId = p.addUserDebugParameter("restitution",0,1,1)
restitutionVelocityThresholdId = p.addUserDebugParameter("res. vel. threshold",0,3,0.2)

116
examples/pybullet/pybullet.c
View File

@@ -5668,6 +5668,114 @@ static PyObject* pybullet_getQuaternionFromEuler(PyObject* self,
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_multiplyTransforms(PyObject* self,
PyObject* args, PyObject* keywds)
{
PyObject* posAObj = 0;
PyObject* ornAObj = 0;
PyObject* posBObj = 0;
PyObject* ornBObj = 0;
int hasPosA=0;
int hasOrnA=0;
int hasPosB=0;
int hasOrnB=0;
double posA[3];
double ornA[4] = {0, 0, 0, 1};
double posB[3];
double ornB[4] = {0, 0, 0, 1};
static char* kwlist[] = {"positionA", "orientationA", "positionB", "orientationB", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOOO", kwlist, &posAObj, &ornAObj,&posBObj, &ornBObj))
{
return NULL;
}
hasPosA = pybullet_internalSetVectord(posAObj, posA);
hasOrnA = pybullet_internalSetVector4d(ornAObj, ornA);
hasPosB = pybullet_internalSetVectord(posBObj, posB);
hasOrnB= pybullet_internalSetVector4d(ornBObj, ornB);
if (hasPosA&&hasOrnA&&hasPosB&&hasOrnB)
{
double outPos[3];
double outOrn[4];
int i;
PyObject* pyListOutObj=0;
PyObject* pyPosOutObj=0;
PyObject* pyOrnOutObj=0;
b3MultiplyTransforms(posA,ornA,posB,ornB, outPos, outOrn);
pyListOutObj = PyTuple_New(2);
pyPosOutObj = PyTuple_New(3);
pyOrnOutObj = PyTuple_New(4);
for (i = 0; i < 3; i++)
PyTuple_SetItem(pyPosOutObj, i, PyFloat_FromDouble(outPos[i]));
for (i = 0; i < 4; i++)
PyTuple_SetItem(pyOrnOutObj, i, PyFloat_FromDouble(outOrn[i]));
PyTuple_SetItem(pyListOutObj, 0, pyPosOutObj);
PyTuple_SetItem(pyListOutObj, 1, pyOrnOutObj);
return pyListOutObj;
}
PyErr_SetString(SpamError, "Invalid input: expected positionA [x,y,z], orientationA [x,y,z,w], positionB, orientationB.");
return NULL;
}
static PyObject* pybullet_invertTransform(PyObject* self,
PyObject* args, PyObject* keywds)
{
PyObject* posObj = 0;
PyObject* ornObj = 0;
double pos[3];
double orn[4] = {0, 0, 0, 1};
int hasPos =0;
int hasOrn =0;
static char* kwlist[] = {"position", "orientation", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO", kwlist, &posObj, &ornObj))
{
return NULL;
}
hasPos = pybullet_internalSetVectord(posObj, pos);
hasOrn = pybullet_internalSetVector4d(ornObj, orn);
if (hasPos && hasOrn)
{
double outPos[3];
double outOrn[4];
int i;
PyObject* pyListOutObj=0;
PyObject* pyPosOutObj=0;
PyObject* pyOrnOutObj=0;
b3InvertTransform(pos, orn, outPos, outOrn);
pyListOutObj = PyTuple_New(2);
pyPosOutObj = PyTuple_New(3);
pyOrnOutObj = PyTuple_New(4);
for (i = 0; i < 3; i++)
PyTuple_SetItem(pyPosOutObj, i, PyFloat_FromDouble(outPos[i]));
for (i = 0; i < 4; i++)
PyTuple_SetItem(pyOrnOutObj, i, PyFloat_FromDouble(outOrn[i]));
PyTuple_SetItem(pyListOutObj, 0, pyPosOutObj);
PyTuple_SetItem(pyListOutObj, 1, pyOrnOutObj);
return pyListOutObj;
}
PyErr_SetString(SpamError, "Invalid input: expected position [x,y,z] and orientation [x,y,z,w].");
return NULL;
}
/// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
/// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
@@ -5774,7 +5882,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
}
{
double pos[3];
double ori[4] = {0, 1.0, 0, 0};
double ori[4] = {0, 0, 0, 1};
int hasPos = pybullet_internalSetVectord(targetPosObj, pos);
int hasOrn = pybullet_internalSetVector4d(targetOrnObj, ori);
@@ -6286,6 +6394,12 @@ static PyMethodDef SpamMethods[] = {
"Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF "
"convention"},
{"multiplyTransforms", (PyCFunction) pybullet_multiplyTransforms, METH_VARARGS | METH_KEYWORDS,
"Multiply two transform, provided as [position], [quaternion]."},
{"invertTransform", (PyCFunction) pybullet_invertTransform, METH_VARARGS | METH_KEYWORDS,
"Invert a transform, provided as [position], [quaternion]."},
{"getMatrixFromQuaternion", pybullet_getMatrixFromQuaternion, METH_VARARGS,
"Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"},