Fix heightmap bug

programs/heightmap.c

@@ -1,17 +1,19 @@
 #define CAMERA_STEP 200
+#define GRID_SIZE 1000
 
-#include "helper.h"
+#define HEIGHTMAP_3D_RESOLUTION 32
+#define HEIGHTMAP_3D_STEP GRID_SIZE
 
-#define GRID_SIZE 3000
+#include "helper.h"
 
 TPE_Unit height(int32_t x, int32_t y)
 {
-  x *= 64;
-  y *= 64;
+  x *= 8;
+  y *= 8;
 
   return 
    TPE_sin(x + TPE_cos(y * 2)) * TPE_sin(y * 2 + TPE_cos(x * 4)) /
-    (TPE_FRACTIONS_PER_UNIT / 4);
+    (TPE_FRACTIONS_PER_UNIT / 2);
 }
 
 TPE_Vec3 environmentDistance(TPE_Vec3 p, TPE_Unit maxD)
@@ -34,6 +36,13 @@ TPE_vec3(0,0,0),GRID_SIZE,height,maxD);
 int main(void)
 {
 
+#if 0
+
+  TPE_Vec3 aaa = TPE_envHeightmap(TPE_vec3(-4759,5492,1120 ),
+TPE_vec3(0,0,0),GRID_SIZE,height,50000);
+
+// WHY Z 1000 ???? SHOULD BE 0
+
 /*
 printf("%d\n",
  TPE_testClosestPointFunction(
@@ -44,13 +53,20 @@ environmentDistance,
   30,
   0)
 );
+*/
 
+TPE_PRINTF_VEC3(aaa);
+printf("\n");
 
 return 0;
-*/
+#endif
 
   helper_init();
 
+for (int y = 0; y < HEIGHTMAP_3D_RESOLUTION; ++y)
+  for (int x = 0; x < HEIGHTMAP_3D_RESOLUTION; ++x)
+    helper_setHeightmapPoint(x,y,height(x - HEIGHTMAP_3D_RESOLUTION / 2,y - HEIGHTMAP_3D_RESOLUTION / 2));
+
   helper_debugDrawOn = 1;
 
   tpe_world.environmentFunction = environmentDistance;
@@ -77,22 +93,12 @@ helper_printCamera();
     (s3l_scene.camera.transform.translation.z < 0));
 }
 
+helper_drawModel(&heightmapModel,
+TPE_vec3(-GRID_SIZE / 2,0,-GRID_SIZE / 2),TPE_vec3(512,512,512),TPE_vec3(0,0,0));
+
     if (helper_debugDrawOn)
       helper_debugDraw(1);
 
-TPE_Vec3 ppp =
-  TPE_envHeightmap(
-TPE_vec3(
-s3l_scene.camera.transform.translation.x,
-s3l_scene.camera.transform.translation.y,
-s3l_scene.camera.transform.translation.z
-)
-,TPE_vec3(0,0,0),GRID_SIZE,height,50000);
-    
-helper_drawPoint3D(ppp,0,255,0);
-
-
-
 for (int yy = -5; yy <= 5; ++yy)
   for (int xx = -5; xx <= 5; ++xx)
   {
@@ -101,6 +107,11 @@ for (int yy = -5; yy <= 5; ++yy)
 
   } 
 
+
+
+
+
+
     helper_frameEnd();
   }
 

programs/helper.h

@@ -59,6 +59,17 @@
   #define S3L_NEAR_CROSS_STRATEGY 1
 #endif
 
+#ifndef HEIGHTMAP_3D_RESOLUTION
+  #define HEIGHTMAP_3D_RESOLUTION 8
+#endif
+
+#ifndef HEIGHTMAP_3D_STEP
+  #define HEIGHTMAP_3D_STEP 512
+#endif
+
+#define HEIGHTMAP_3D_POINTS (HEIGHTMAP_3D_RESOLUTION * HEIGHTMAP_3D_RESOLUTION)
+#define HEIGHTMAP_3D_TRIANGLES (((HEIGHTMAP_3D_RESOLUTION - 1) * (HEIGHTMAP_3D_RESOLUTION - 1) * 2) * 3)
+
 #define S3L_USE_WIDER_TYPES 1
 
 #include "small3dlib.h"
@@ -67,6 +78,10 @@
 
 #define helper_lastBody tpe_world.bodies[tpe_world.bodyCount - 1]
 
+S3L_Unit heightmapVertices[HEIGHTMAP_3D_POINTS * 3];
+S3L_Index heightmapTriangles[HEIGHTMAP_3D_TRIANGLES];
+S3L_Model3D heightmapModel;
+
 S3L_Unit cubeVertices[] = { S3L_CUBE_VERTICES(TPE_FRACTIONS_PER_UNIT) };  
 S3L_Index cubeTriangles[] = { S3L_CUBE_TRIANGLES };
 S3L_Model3D cubeModel;
@@ -582,7 +597,7 @@ void s3l_drawPixel(S3L_PixelInfo *p)
 {
   if (p->triangleIndex != s3l_previousTriangleID)
   {
-    S3L_Index *v = _helper_drawnModel->triangles 
+    const S3L_Index *v = _helper_drawnModel->triangles 
       + 3 * p->triangleIndex;
 
     TPE_Vec3 a = TPE_vec3(
@@ -730,6 +745,21 @@ void helper_draw3DSphereInside(TPE_Vec3 pos, TPE_Vec3 scale, TPE_Vec3 rot)
   helper_drawModel(&sphereModel,pos,scale,rot);
 }
 
+TPE_Vec3 helper_heightmapPointLocation(int index)
+{
+  return TPE_vec3(
+    (-1 * HEIGHTMAP_3D_RESOLUTION * HEIGHTMAP_3D_STEP) / 2 + (index % HEIGHTMAP_3D_RESOLUTION) * HEIGHTMAP_3D_STEP + HEIGHTMAP_3D_STEP / 2,0,
+    (-1 * HEIGHTMAP_3D_RESOLUTION * HEIGHTMAP_3D_STEP) / 2 + (index / HEIGHTMAP_3D_RESOLUTION) * HEIGHTMAP_3D_STEP + HEIGHTMAP_3D_STEP / 2);
+}
+
+void helper_setHeightmapPoint(uint16_t x, uint16_t y, TPE_Unit height)
+{
+  x = x % HEIGHTMAP_3D_RESOLUTION;
+  y = y % HEIGHTMAP_3D_RESOLUTION;
+
+  heightmapVertices[(y * HEIGHTMAP_3D_RESOLUTION + x) * 3 + 1] = height;
+}
+
 void helper_init(void)
 {
   helper_lightDir = TPE_vec3Normalized(TPE_vec3(300,200,100));
@@ -759,6 +789,48 @@ void helper_init(void)
 
   S3L_model3DInit(triangleVertices,3,triangleTriangles,2,&triangleModel);
 
+  // build the heightmap 3D model:
+
+  for (int i = 0; i < HEIGHTMAP_3D_POINTS; ++i)
+  {
+    TPE_Vec3 pos = helper_heightmapPointLocation(i);
+
+    heightmapVertices[i * 3] = pos.x;
+    heightmapVertices[i * 3 + 1] = pos.y;
+    heightmapVertices[i * 3 + 2] = pos.z;
+  }
+
+  int index = 0;
+
+  for (int j = 0; j < HEIGHTMAP_3D_RESOLUTION - 1; ++j)
+    for (int i = 0; i < HEIGHTMAP_3D_RESOLUTION - 1; ++i)
+    {
+      heightmapTriangles[index] = j * HEIGHTMAP_3D_RESOLUTION + i;
+      heightmapTriangles[index + 1] = heightmapTriangles[index] + 1;
+      heightmapTriangles[index + 2] = heightmapTriangles[index] + HEIGHTMAP_3D_RESOLUTION;
+
+      heightmapTriangles[index + 3] = heightmapTriangles[index + 1];
+      heightmapTriangles[index + 4] = heightmapTriangles[index + 1] + HEIGHTMAP_3D_RESOLUTION;
+      heightmapTriangles[index + 5] = heightmapTriangles[index + 4] - 1;
+/*
+      heightmapTriangles[index] = j * HEIGHTMAP_3D_RESOLUTION + i;
+      heightmapTriangles[index + 1] = heightmapTriangles[index] + 1;
+      heightmapTriangles[index + 2] = heightmapTriangles[index + 1] + HEIGHTMAP_3D_RESOLUTION;
+
+      heightmapTriangles[index + 3] = heightmapTriangles[index];
+      heightmapTriangles[index + 4] = heightmapTriangles[index + 1] + HEIGHTMAP_3D_RESOLUTION;
+      heightmapTriangles[index + 5] = heightmapTriangles[index] + HEIGHTMAP_3D_RESOLUTION;
+*/
+      index += 6;
+    }
+
+  S3L_model3DInit(
+    heightmapVertices,
+    HEIGHTMAP_3D_POINTS * 3,
+    heightmapTriangles,
+    ((HEIGHTMAP_3D_RESOLUTION - 1) * (HEIGHTMAP_3D_RESOLUTION - 1) * 2),
+    &heightmapModel);
+
   S3L_sceneInit(0,1,&s3l_scene);
 
   TPE_worldInit(&tpe_world,tpe_bodies,0,0);

tinyphysicsengine.h

@@ -3101,8 +3101,8 @@ TPE_Vec3 TPE_envHeightmap(TPE_Vec3 point, TPE_Vec3 center, TPE_Unit gridSize,
         squareY++; 
 
         bl = tl; br = tr;
-        tl = TPE_vec3(tl.x,heightFunction(squareX,squareY + 1),tl.z + gridSize);
-        tr = TPE_vec3(tr.x,heightFunction(squareX + 1,squareY + 1),tl.z + gridSize);
+        tl = TPE_vec3(bl.x,heightFunction(squareX,squareY + 1),bl.z + gridSize);
+        tr = TPE_vec3(br.x,heightFunction(squareX + 1,squareY + 1),bl.z + gridSize);
 
         break;
 
@@ -3110,8 +3110,8 @@ TPE_Vec3 TPE_envHeightmap(TPE_Vec3 point, TPE_Vec3 center, TPE_Unit gridSize,
         squareX++;
 
         bl = br; tl = tr;
-        tr = TPE_vec3(tr.x + gridSize,heightFunction(squareX + 1,squareY + 1),tr.z);
-        br = TPE_vec3(br.x + gridSize,heightFunction(squareX + 1,squareY),br.z);
+        tr = TPE_vec3(tl.x + gridSize,heightFunction(squareX + 1,squareY + 1),tl.z);
+        br = TPE_vec3(bl.x + gridSize,heightFunction(squareX + 1,squareY),bl.z);
 
         break;
 
@@ -3119,8 +3119,8 @@ TPE_Vec3 TPE_envHeightmap(TPE_Vec3 point, TPE_Vec3 center, TPE_Unit gridSize,
         squareY--;
 
         tl = bl; tr = br;
-        bl = TPE_vec3(bl.x,heightFunction(squareX,squareY),bl.z - gridSize);
-        br = TPE_vec3(br.x,heightFunction(squareX + 1,squareY),br.z - gridSize);
+        bl = TPE_vec3(tl.x,heightFunction(squareX,squareY),tl.z - gridSize);
+        br = TPE_vec3(tr.x,heightFunction(squareX + 1,squareY),tr.z - gridSize);
 
         break;
 
@@ -3128,8 +3128,8 @@ TPE_Vec3 TPE_envHeightmap(TPE_Vec3 point, TPE_Vec3 center, TPE_Unit gridSize,
         squareX--;
 
         br = bl; tr = tl;
-        tl = TPE_vec3(tl.x - gridSize,heightFunction(squareX,squareY + 1),tl.z);
-        bl = TPE_vec3(bl.x - gridSize,heightFunction(squareX,squareY),bl.z);
+        tl = TPE_vec3(tr.x - gridSize,heightFunction(squareX,squareY + 1),tr.z);
+        bl = TPE_vec3(br.x - gridSize,heightFunction(squareX,squareY),br.z);
 
         break;