/* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006,2008 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #include "HeightfieldExample.h" // always include our own header first! #include "btBulletDynamicsCommon.h" #include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" #include "../CommonInterfaces/CommonRigidBodyBase.h" #include "../MultiThreadedDemo/CommonRigidBodyMTBase.h" #include "../CommonInterfaces/CommonParameterInterface.h" #include "../OpenGLWindow/GLInstanceGraphicsShape.h" #include "../Utils/b3BulletDefaultFileIO.h" #include "../Importers/ImportURDFDemo/urdfStringSplit.h" #include "stb_image/stb_image.h" // constants ------------------------------------------------------------------- static const btScalar s_gravity = 9.8; // 9.8 m/s^2 static int s_gridSize = 16 + 1; // must be (2^N) + 1 static btScalar s_gridSpacing = 0.5; static btScalar s_gridHeightScale = 0.02; // the singularity at the center of the radial model means we need a lot of // finely-spaced time steps to get the physics right. // These numbers are probably too aggressive for a real game! // delta phase: radians per second static const btScalar s_deltaPhase = 0.25 * 2.0 * SIMD_PI; // what type of terrain is generated? enum eTerrainModel { eRadial = 0, // deterministic eFractal = 1, // random eCSVFile = 2,//csv file used in DeepLoco for example eImageFile = 3,//terrain from png/jpg files, asset from https://www.beamng.com/threads/tutorial-adding-heightmap-roads-using-blender.16356/ }; typedef unsigned char byte_t; //////////////////////////////////////////////////////////////////////////////// // // static helper methods // // Only used within this file (helpers and terrain generation, etc) // //////////////////////////////////////////////////////////////////////////////// static const char * getTerrainTypeName ( eTerrainModel model ) { switch (model) { case eRadial: return "Radial"; case eFractal: return "Fractal"; case eCSVFile: return "DeepLocoCSV"; case eImageFile: return "Image"; default: btAssert(!"bad terrain model type"); } return NULL; } static btVector3 getUpVector ( int upAxis, btScalar regularValue, btScalar upValue ) { btAssert(upAxis >= 0 && upAxis <= 2 && "bad up axis"); btVector3 v(regularValue, regularValue, regularValue); v[upAxis] = upValue; return v; } // TODO: it would probably cleaner to have a struct per data type, so // you could lookup byte sizes, conversion functions, etc. static int getByteSize ( PHY_ScalarType type ) { int size = 0; switch (type) { case PHY_FLOAT: size = sizeof(btScalar); break; case PHY_UCHAR: size = sizeof(unsigned char); break; case PHY_SHORT: size = sizeof(short); break; default: btAssert(!"Bad heightfield data type"); } return size; } static btScalar convertToFloat ( const byte_t * p, PHY_ScalarType type ) { btAssert(p); switch (type) { case PHY_FLOAT: { btScalar * pf = (btScalar *)p; return *pf; } case PHY_UCHAR: { unsigned char * pu = (unsigned char *)p; return ((*pu) * s_gridHeightScale); } case PHY_SHORT: { short * ps = (short *)p; return ((*ps) * s_gridHeightScale); } default: btAssert(!"bad type"); } return 0; } static btScalar getGridHeight ( byte_t * grid, int i, int j, PHY_ScalarType type ) { btAssert(grid); btAssert(i >= 0 && i < s_gridSize); btAssert(j >= 0 && j < s_gridSize); int bpe = getByteSize(type); btAssert(bpe > 0 && "bad bytes per element"); int idx = (j * s_gridSize) + i; long offset = ((long)bpe) * idx; byte_t * p = grid + offset; return convertToFloat(p, type); } static void convertFromFloat ( byte_t * p, btScalar value, PHY_ScalarType type ) { btAssert(p && "null"); switch (type) { case PHY_FLOAT: { btScalar * pf = (btScalar *)p; *pf = value; } break; case PHY_UCHAR: { unsigned char * pu = (unsigned char *)p; *pu = (unsigned char)(value / s_gridHeightScale); } break; case PHY_SHORT: { short * ps = (short *)p; *ps = (short)(value / s_gridHeightScale); } break; default: btAssert(!"bad type"); } } // creates a radially-varying heightfield static void setRadial ( byte_t * grid, int bytesPerElement, PHY_ScalarType type, btScalar phase = 0.0 ) { btAssert(grid); btAssert(bytesPerElement > 0); // min/max btScalar period = 0.5 / s_gridSpacing; btScalar floor = 0.0; btScalar min_r = 3.0 * btSqrt(s_gridSpacing); btScalar magnitude = 5.0 * btSqrt(s_gridSpacing); // pick a base_phase such that phase = 0 results in max height // (this way, if you create a heightfield with phase = 0, // you can rely on the min/max heights that result) btScalar base_phase = (0.5 * SIMD_PI) - (period * min_r); phase += base_phase; // center of grid btScalar cx = 0.5 * s_gridSize * s_gridSpacing; btScalar cy = cx; // assume square grid byte_t * p = grid; for (int i = 0; i < s_gridSize; ++i) { float x = i * s_gridSpacing; for (int j = 0; j < s_gridSize; ++j) { float y = j * s_gridSpacing; float dx = x - cx; float dy = y - cy; float r = sqrt((dx * dx) + (dy * dy)); float z = period; if (r < min_r) { r = min_r; } z = (1.0 / r) * sin(period * r + phase); if (z > period) { z = period; } else if (z < -period) { z = -period; } z = floor + magnitude * z; convertFromFloat(p, z, type); p += bytesPerElement; } } } static float randomHeight ( int step ) { return (0.33 * s_gridSpacing * s_gridSize * step * (rand() - (0.5 * RAND_MAX))) / (1.0 * RAND_MAX * s_gridSize); } #if 0 static void dumpGrid ( const byte_t * grid, int bytesPerElement, PHY_ScalarType type, int max ) { //std::cerr << "Grid:\n"; char buffer[32]; for (int j = 0; j < max; ++j) { for (int i = 0; i < max; ++i) { long offset = j * s_gridSize + i; float z = convertToFloat(grid + offset * bytesPerElement, type); sprintf(buffer, "%6.2f", z); //std::cerr << " " << buffer; } //std::cerr << "\n"; } } #endif static void updateHeight ( byte_t * p, btScalar new_val, PHY_ScalarType type ) { btScalar old_val = convertToFloat(p, type); if (!old_val) { convertFromFloat(p, new_val, type); } } // creates a random, fractal heightfield static void setFractal ( byte_t * grid, int bytesPerElement, PHY_ScalarType type, int step ) { btAssert(grid); btAssert(bytesPerElement > 0); btAssert(step > 0); btAssert(step < s_gridSize); int newStep = step / 2; // std::cerr << "Computing grid with step = " << step << ": before\n"; // dumpGrid(grid, bytesPerElement, type, step + 1); // special case: starting (must set four corners) if (s_gridSize - 1 == step) { // pick a non-zero (possibly negative) base elevation for testing btScalar base = randomHeight(step / 2); convertFromFloat(grid, base, type); convertFromFloat(grid + step * bytesPerElement, base, type); convertFromFloat(grid + step * s_gridSize * bytesPerElement, base, type); convertFromFloat(grid + (step * s_gridSize + step) * bytesPerElement, base, type); } // determine elevation of each corner btScalar c00 = convertToFloat(grid, type); btScalar c01 = convertToFloat(grid + step * bytesPerElement, type); btScalar c10 = convertToFloat(grid + (step * s_gridSize) * bytesPerElement, type); btScalar c11 = convertToFloat(grid + (step * s_gridSize + step) * bytesPerElement, type); // set top middle updateHeight(grid + newStep * bytesPerElement, 0.5 * (c00 + c01) + randomHeight(step), type); // set left middle updateHeight(grid + (newStep * s_gridSize) * bytesPerElement, 0.5 * (c00 + c10) + randomHeight(step), type); // set right middle updateHeight(grid + (newStep * s_gridSize + step) * bytesPerElement, 0.5 * (c01 + c11) + randomHeight(step), type); // set bottom middle updateHeight(grid + (step * s_gridSize + newStep) * bytesPerElement, 0.5 * (c10 + c11) + randomHeight(step), type); // set middle updateHeight(grid + (newStep * s_gridSize + newStep) * bytesPerElement, 0.25 * (c00 + c01 + c10 + c11) + randomHeight(step), type); // std::cerr << "Computing grid with step = " << step << ": after\n"; // dumpGrid(grid, bytesPerElement, type, step + 1); // terminate? if (newStep < 2) { return; } // recurse setFractal(grid, bytesPerElement, type, newStep); setFractal(grid + newStep * bytesPerElement, bytesPerElement, type, newStep); setFractal(grid + (newStep * s_gridSize) * bytesPerElement, bytesPerElement, type, newStep); setFractal(grid + ((newStep * s_gridSize) + newStep) * bytesPerElement, bytesPerElement, type, newStep); } #define MYLINELENGTH 16*32768 static byte_t * getRawHeightfieldData ( eTerrainModel model, PHY_ScalarType type, btScalar& minHeight, btScalar& maxHeight ) { if (model==eImageFile) { b3BulletDefaultFileIO fileIO; char relativeFileName[1024]; int found = fileIO.findFile("heightmaps/wm_height_out.png", relativeFileName, 1024); b3AlignedObjectArray buffer; buffer.reserve(1024); int fileId = fileIO.fileOpen(relativeFileName,"rb"); if (fileId>=0) { int size = fileIO.getFileSize(fileId); if (size>0) { buffer.resize(size); int actual = fileIO.fileRead(fileId,&buffer[0],size); if (actual != size) { b3Warning("STL filesize mismatch!\n"); buffer.resize(0); } } fileIO.fileClose(fileId); } if (buffer.size()) { int width, height,n; unsigned char* image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3); if (image) { printf("width=%d, height=%d at %d channels\n", width,height, n); s_gridSize = width; s_gridSpacing = 0.2; s_gridHeightScale = 0.2; fileIO.fileClose(fileId); long nElements = ((long)s_gridSize) * s_gridSize; // std::cerr << " nElements = " << nElements << "\n"; int bytesPerElement = getByteSize(type); // std::cerr << " bytesPerElement = " << bytesPerElement << "\n"; btAssert(bytesPerElement > 0 && "bad bytes per element"); long nBytes = nElements * bytesPerElement; // std::cerr << " nBytes = " << nBytes << "\n"; byte_t * raw = new byte_t[nBytes]; btAssert(raw && "out of memory"); byte_t * p = raw; for (int j = 0; j < width; ++j) { for (int i = 0; i < width; ++i) { float x = i * s_gridSpacing; float y = j * s_gridSpacing; float heightScaling = (14. / 256.); float z = double(image[(width - 1 - i) * 3 + width*j * 3]) * heightScaling; convertFromFloat(p, z, type); // update min/max if (!i && !j) { minHeight = z; maxHeight = z; } else { if (z < minHeight) { minHeight = z; } if (z > maxHeight) { maxHeight = z; } } p += bytesPerElement; } } return raw; } } } if (model==eCSVFile) { { b3BulletDefaultFileIO fileIO; char relativePath[1024]; int found = fileIO.findFile("heightmaps/ground0.txt", relativePath, 1024); char lineBuffer[MYLINELENGTH]; int slot = fileIO.fileOpen(relativePath, "r"); int rows = 0; int cols=0; btAlignedObjectArray allValues; if (slot>=0) { char* lineChar; while (lineChar = fileIO.readLine(slot, lineBuffer, MYLINELENGTH)) { rows=0; char** values = urdfStrSplit(lineChar, ","); if (values) { int index = 0; char* value; while (value = values[index++]) { std::string strval(value); double v; if(sscanf(value, "%lf", &v) == 1) { //printf("strlen = %d\n", strval.length()); //printf("value[%d,%d]=%s or (%f)", cols,rows,value, v); allValues.push_back(v); rows++; } } } cols++; } printf("done, rows=%d, cols=%d\n", rows, cols); int width = rows-1; s_gridSize = rows; s_gridSpacing = 0.2; s_gridHeightScale = 0.2; fileIO.fileClose(slot); long nElements = ((long)s_gridSize) * s_gridSize; // std::cerr << " nElements = " << nElements << "\n"; int bytesPerElement = getByteSize(type); // std::cerr << " bytesPerElement = " << bytesPerElement << "\n"; btAssert(bytesPerElement > 0 && "bad bytes per element"); long nBytes = nElements * bytesPerElement; // std::cerr << " nBytes = " << nBytes << "\n"; byte_t * raw = new byte_t[nBytes]; btAssert(raw && "out of memory"); byte_t * p = raw; for (int i = 0; i < width; ++i) { float x = i * s_gridSpacing; for (int j = 0; j < width; ++j) { float y = j * s_gridSpacing; float z = allValues[i+width*j]; convertFromFloat(p, z, type); // update min/max if (!i && !j) { minHeight = z; maxHeight = z; } else { if (z < minHeight) { minHeight = z; } if (z > maxHeight) { maxHeight = z; } } p += bytesPerElement; } } return raw; } printf("found=%d",found); } } else { if (model==eRadial) { s_gridSize = 16 + 1; // must be (2^N) + 1 s_gridSpacing = 0.5; s_gridHeightScale = 0.02; } else { s_gridSize = 256 + 1; // must be (2^N) + 1 s_gridSpacing = 0.5; s_gridHeightScale = 0.02; } // std::cerr << "\nRegenerating terrain\n"; // std::cerr << " model = " << model << "\n"; // std::cerr << " type = " << type << "\n"; long nElements = ((long)s_gridSize) * s_gridSize; // std::cerr << " nElements = " << nElements << "\n"; int bytesPerElement = getByteSize(type); // std::cerr << " bytesPerElement = " << bytesPerElement << "\n"; btAssert(bytesPerElement > 0 && "bad bytes per element"); long nBytes = nElements * bytesPerElement; // std::cerr << " nBytes = " << nBytes << "\n"; byte_t * raw = new byte_t[nBytes]; btAssert(raw && "out of memory"); // reseed randomization every 30 seconds // srand(time(NULL) / 30); // populate based on model switch (model) { case eRadial: setRadial(raw, bytesPerElement, type); break; case eFractal: for (int i = 0; i < nBytes; i++) { raw[i] = 0; } setFractal(raw, bytesPerElement, type, s_gridSize - 1); break; default: btAssert(!"bad model type"); } // std::cerr << "final grid:\n"; //dumpGrid(raw, bytesPerElement, type, s_gridSize - 1); // find min/max for (int i = 0; i < s_gridSize; ++i) { for (int j = 0; j < s_gridSize; ++j) { btScalar z = getGridHeight(raw, i, j, type); // std::cerr << "i=" << i << ", j=" << j << ": z=" << z << "\n"; // update min/max if (!i && !j) { minHeight = z; maxHeight = z; } else { if (z < minHeight) { minHeight = z; } if (z > maxHeight) { maxHeight = z; } } } } if (maxHeight < -minHeight) { maxHeight = -minHeight; } if (minHeight > -maxHeight) { minHeight = -maxHeight; } // std::cerr << " minHeight = " << minHeight << "\n"; // std::cerr << " maxHeight = " << maxHeight << "\n"; return raw; } return 0; } //////////////////////////////////////////////////////////////////////////////// // // TerrainDemo class // //////////////////////////////////////////////////////////////////////////////// /// class that demonstrates the btHeightfieldTerrainShape object class HeightfieldExample : public CommonRigidBodyMTBase//CommonRigidBodyBase { public: // constructor, destructor --------------------------------------------- HeightfieldExample(struct GUIHelperInterface* helper); virtual ~HeightfieldExample(); virtual void initPhysics(); // public class methods ------------------------------------------------ void castRays(); void stepSimulation(float deltaTime); void resetCamera() { float dist = 15; float pitch = -32; float yaw = 35; float targetPos[3] = { 0, 0, 0 }; m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } private: // private helper methods ---------------------------------------------- void resetPhysics(void); void clearWorld(void); // private data members ------------------------------------------------ int m_upAxis; PHY_ScalarType m_type; eTerrainModel m_model; byte_t * m_rawHeightfieldData; btScalar m_minHeight; btScalar m_maxHeight; float m_phase; // for dynamics bool m_isDynamic; btHeightfieldTerrainShape * m_heightfieldShape; }; #define HEIGHTFIELD_TYPE_COUNT 4 eTerrainModel gHeightfieldType = eRadial; void setHeightfieldTypeComboBoxCallback(int combobox, const char* item, void* userPointer) { const char** items = static_cast(userPointer); for (int i = 0; i < HEIGHTFIELD_TYPE_COUNT; ++i) { if (strcmp(item, items[i]) == 0) { gHeightfieldType = static_cast(i); break; } } } HeightfieldExample::HeightfieldExample(struct GUIHelperInterface* helper) : CommonRigidBodyMTBase(helper), m_upAxis(1), m_type(PHY_FLOAT), m_model(eFractal), m_rawHeightfieldData(NULL), m_phase(0.0), m_isDynamic(true), m_heightfieldShape(0) { { // create a combo box for selecting the solver type static const char* sHeightfieldTypeComboBoxItems[HEIGHTFIELD_TYPE_COUNT]; for (int i = 0; i < HEIGHTFIELD_TYPE_COUNT; ++i) { eTerrainModel heightfieldType = static_cast(i); sHeightfieldTypeComboBoxItems[i] = getTerrainTypeName(heightfieldType); } ComboBoxParams comboParams; comboParams.m_userPointer = sHeightfieldTypeComboBoxItems; comboParams.m_numItems = HEIGHTFIELD_TYPE_COUNT; comboParams.m_startItem = gHeightfieldType; comboParams.m_items = sHeightfieldTypeComboBoxItems; comboParams.m_callback = setHeightfieldTypeComboBoxCallback; m_guiHelper->getParameterInterface()->registerComboBox(comboParams); } } HeightfieldExample::~HeightfieldExample(void) { clearWorld(); } class MyTriangleCollector3 : public btTriangleCallback { public: btAlignedObjectArray* m_pVerticesOut; btAlignedObjectArray* m_pIndicesOut; MyTriangleCollector3() { m_pVerticesOut = 0; m_pIndicesOut = 0; } virtual void processTriangle(btVector3* tris, int partId, int triangleIndex) { for (int k = 0; k < 3; k++) { GLInstanceVertex v; v.xyzw[3] = 0; v.uv[0] = v.uv[1] = 0.5f; btVector3 normal = (tris[0] - tris[1]).cross(tris[0] - tris[2]); normal.safeNormalize(); for (int l = 0; l < 3; l++) { v.xyzw[l] = tris[k][l]; v.normal[l] = normal[l]; } m_pIndicesOut->push_back(m_pVerticesOut->size()); m_pVerticesOut->push_back(v); } } }; #define NUMRAYS2 500 #define USE_PARALLEL_RAYCASTS 1 class btRaycastBar3 { public: btVector3 source[NUMRAYS2]; btVector3 dest[NUMRAYS2]; btVector3 direction[NUMRAYS2]; btVector3 hit[NUMRAYS2]; btVector3 normal[NUMRAYS2]; struct GUIHelperInterface* m_guiHelper; int frame_counter; int ms; int sum_ms; int sum_ms_samples; int min_ms; int max_ms; #ifdef USE_BT_CLOCK btClock frame_timer; #endif //USE_BT_CLOCK btScalar dx; btScalar min_x; btScalar max_x; btScalar max_y; btScalar sign; btRaycastBar3() { m_guiHelper = 0; ms = 0; max_ms = 0; min_ms = 9999; sum_ms_samples = 0; sum_ms = 0; } btRaycastBar3(btScalar ray_length, btScalar z, btScalar max_y, struct GUIHelperInterface* guiHelper, int upAxisIndex) { m_guiHelper = guiHelper; frame_counter = 0; ms = 0; max_ms = 0; min_ms = 9999; sum_ms_samples = 0; sum_ms = 0; dx = 10.0; min_x = 0; max_x = 0; this->max_y = max_y; sign = 1.0; btScalar dalpha = 2 * SIMD_2_PI / NUMRAYS2; for (int i = 0; i < NUMRAYS2; i++) { btScalar alpha = dalpha * i; // rotate around by alpha degrees y btVector3 upAxis(0, 0, 0); upAxis[upAxisIndex] = 1; btQuaternion q(upAxis, alpha); direction[i] = btVector3(1.0, 0.0, 0.0); direction[i] = quatRotate(q, direction[i]); direction[i] = direction[i] * ray_length; if (upAxisIndex == 1) { source[i] = btVector3(min_x, max_y, z); } else { source[i] = btVector3(min_x, z, max_y); } dest[i] = source[i] + direction[i]; dest[i][upAxisIndex] = -1000; normal[i] = btVector3(1.0, 0.0, 0.0); } } void move(btScalar dt) { if (dt > btScalar(1.0 / 60.0)) dt = btScalar(1.0 / 60.0); for (int i = 0; i < NUMRAYS2; i++) { source[i][0] += dx * dt * sign; dest[i][0] += dx * dt * sign; } if (source[0][0] < min_x) sign = 1.0; else if (source[0][0] > max_x) sign = -1.0; } void castRays(btCollisionWorld* cw, int iBegin, int iEnd) { if (m_guiHelper==0) return; for (int i = iBegin; i < iEnd; ++i) { btCollisionWorld::ClosestRayResultCallback cb(source[i], dest[i]); { BT_PROFILE("cw->rayTest"); //to disable raycast accelerator, uncomment next line //cb.m_flags |= btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator; cw->rayTest(source[i], dest[i], cb); } if (cb.hasHit()) { hit[i] = cb.m_hitPointWorld; normal[i] = cb.m_hitNormalWorld; normal[i].normalize(); } else { hit[i] = dest[i]; normal[i] = btVector3(1.0, 0.0, 0.0); } } } struct CastRaysLoopBody : public btIParallelForBody { btCollisionWorld* mWorld; btRaycastBar3* mRaycasts; CastRaysLoopBody(btCollisionWorld* cw, btRaycastBar3* rb) : mWorld(cw), mRaycasts(rb) {} void forLoop(int iBegin, int iEnd) const { mRaycasts->castRays(mWorld, iBegin, iEnd); } }; void cast(btCollisionWorld* cw, bool multiThreading = false) { BT_PROFILE("cast"); #ifdef USE_BT_CLOCK frame_timer.reset(); #endif //USE_BT_CLOCK #ifdef BATCH_RAYCASTER if (!gBatchRaycaster) return; gBatchRaycaster->clearRays(); for (int i = 0; i < NUMRAYS; i++) { gBatchRaycaster->addRay(source[i], dest[i]); } gBatchRaycaster->performBatchRaycast(); for (int i = 0; i < gBatchRaycaster->getNumRays(); i++) { const SpuRaycastTaskWorkUnitOut& out = (*gBatchRaycaster)[i]; hit[i].setInterpolate3(source[i], dest[i], out.hitFraction); normal[i] = out.hitNormal; normal[i].normalize(); } #else #if USE_PARALLEL_RAYCASTS if (multiThreading) { CastRaysLoopBody rayLooper(cw, this); int grainSize = 20; // number of raycasts per task btParallelFor(0, NUMRAYS2, grainSize, rayLooper); } else #endif // USE_PARALLEL_RAYCASTS { // single threaded castRays(cw, 0, NUMRAYS2); } #ifdef USE_BT_CLOCK ms += frame_timer.getTimeMilliseconds(); #endif //USE_BT_CLOCK frame_counter++; if (frame_counter > 50) { min_ms = ms < min_ms ? ms : min_ms; max_ms = ms > max_ms ? ms : max_ms; sum_ms += ms; sum_ms_samples++; btScalar mean_ms = (btScalar)sum_ms / (btScalar)sum_ms_samples; printf("%d rays in %d ms %d %d %f\n", NUMRAYS2 * frame_counter, ms, min_ms, max_ms, mean_ms); ms = 0; frame_counter = 0; } #endif } void draw() { if (m_guiHelper) { btAlignedObjectArray indices; btAlignedObjectArray points; float lineColor[4] = { 1, 0.4, .4, 1 }; for (int i = 0; i < NUMRAYS2; i++) { btVector3FloatData s, h; for (int w = 0; w < 4; w++) { s.m_floats[w] = source[i][w]; h.m_floats[w] = hit[i][w]; } points.push_back(s); points.push_back(h); indices.push_back(indices.size()); indices.push_back(indices.size()); } m_guiHelper->getRenderInterface()->drawLines(&points[0].m_floats[0], lineColor, points.size(), sizeof(btVector3FloatData), &indices[0], indices.size(), 1); } } }; static btRaycastBar3 raycastBar; void HeightfieldExample::castRays() { #ifdef BT_THREADSAFE raycastBar.cast(m_dynamicsWorld, true); #else raycastBar.cast(m_dynamicsWorld, false); #endif } void HeightfieldExample::stepSimulation(float deltaTime) { castRays(); raycastBar.draw(); // if dynamic and radial, go ahead and update the field if (m_rawHeightfieldData && m_isDynamic && eRadial == m_model && m_heightfieldShape) { btAlignedObjectArray gfxVertices; btAlignedObjectArray indices; int strideInBytes = 9 * sizeof(float); m_phase += s_deltaPhase * deltaTime; if (m_phase > 2.0 * SIMD_PI) { m_phase -= 2.0 * SIMD_PI; } int bpe = getByteSize(m_type); btAssert(bpe > 0 && "Bad bytes per element"); setRadial(m_rawHeightfieldData, bpe, m_type, m_phase); MyTriangleCollector3 col; col.m_pVerticesOut = &gfxVertices; col.m_pIndicesOut = &indices; btVector3 aabbMin, aabbMax; for (int k = 0; k < 3; k++) { aabbMin[k] = -BT_LARGE_FLOAT; aabbMax[k] = BT_LARGE_FLOAT; } m_heightfieldShape->processAllTriangles(&col, aabbMin, aabbMax); if (gfxVertices.size() && indices.size()) { m_guiHelper->getRenderInterface()->updateShape(m_heightfieldShape->getUserIndex(), &gfxVertices[0].xyzw[0], gfxVertices.size()); } } if (m_model != gHeightfieldType) { m_model = gHeightfieldType; resetPhysics(); } CommonRigidBodyMTBase::stepSimulation(deltaTime); } //////////////////////////////////////////////////////////////////////////////// // // TerrainDemo -- public class methods // //////////////////////////////////////////////////////////////////////////////// /// one-time class and physics initialization void HeightfieldExample::initPhysics() { // std::cerr << "initializing...\n"; createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_upAxis = 2; // start with Y-axis as "up" m_guiHelper->setUpAxis(m_upAxis); raycastBar = btRaycastBar3(2500.0, 0, 2.0, m_guiHelper, m_upAxis); // set up basic state m_type = PHY_FLOAT; m_model = gHeightfieldType; m_isDynamic = true; // set up the physics world // initialize axis- or type-dependent physics from here this->resetPhysics(); } //////////////////////////////////////////////////////////////////////////////// // // TerrainDemo -- private helper methods // //////////////////////////////////////////////////////////////////////////////// /// called whenever key terrain attribute is changed void HeightfieldExample::resetPhysics(void) { m_guiHelper->removeAllGraphicsInstances(); // remove old heightfield clearWorld(); // reset gravity to point in appropriate direction m_dynamicsWorld->setGravity(getUpVector(m_upAxis, 0.0, -s_gravity)); // get new heightfield of appropriate type m_rawHeightfieldData = getRawHeightfieldData(m_model, m_type, m_minHeight, m_maxHeight); btAssert(m_rawHeightfieldData && "failed to create raw heightfield"); bool flipQuadEdges = false; m_heightfieldShape = new btHeightfieldTerrainShape(s_gridSize, s_gridSize, m_rawHeightfieldData, s_gridHeightScale, m_minHeight, m_maxHeight, m_upAxis, m_type, flipQuadEdges); btAssert(m_heightfieldShape && "null heightfield"); // set origin to middle of heightfield btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(0, 0, -4)); if (m_model== eImageFile) { b3BulletDefaultFileIO fileIO; char relativeFileName[1024]; int found = fileIO.findFile("heightmaps/gimp_overlay_out.png", relativeFileName, 1024); b3AlignedObjectArray buffer; buffer.reserve(1024); int fileId = fileIO.fileOpen(relativeFileName, "rb"); if (fileId >= 0) { int size = fileIO.getFileSize(fileId); if (size>0) { buffer.resize(size); int actual = fileIO.fileRead(fileId, &buffer[0], size); if (actual != size) { b3Warning("STL filesize mismatch!\n"); buffer.resize(0); } } fileIO.fileClose(fileId); } if (buffer.size()) { int width, height, n; unsigned char* image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3); if (image) { int texId = m_guiHelper->registerTexture(image, width, height); m_heightfieldShape->setUserIndex2(texId); } } } if (m_upAxis == 2) m_heightfieldShape->setFlipTriangleWinding(true); //buildAccelerator is optional, it may not support all features. m_heightfieldShape->buildAccelerator(); // scale the shape btVector3 localScaling = getUpVector(m_upAxis, s_gridSpacing, 1.0); m_heightfieldShape->setLocalScaling(localScaling); // stash this shape away m_collisionShapes.push_back(m_heightfieldShape); // create ground object float mass = 0.0; btRigidBody* body = createRigidBody(mass, tr, m_heightfieldShape); double color[4]={1,1,1,1}; m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); m_guiHelper->changeRGBAColor(body->getUserIndex(),color); } /// removes all objects and shapes from the world void HeightfieldExample::clearWorld(void) { if (m_dynamicsWorld) { //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) { btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; btRigidBody* body = btRigidBody::upcast(obj); if (body && body->getMotionState()) { delete body->getMotionState(); } m_dynamicsWorld->removeCollisionObject(obj); delete obj; } //delete collision shapes for (int j = 0; j < m_collisionShapes.size(); j++) { btCollisionShape* shape = m_collisionShapes[j]; delete shape; } m_collisionShapes.clear(); // delete raw heightfield data delete[] m_rawHeightfieldData; m_rawHeightfieldData = NULL; } } CommonExampleInterface* HeightfieldExampleCreateFunc(CommonExampleOptions& options) { return new HeightfieldExample(options.m_guiHelper); } B3_STANDALONE_EXAMPLE(HeightfieldExampleCreateFunc)