Imported platform_displacement module

src/engine/math_util.c

@@ -1987,6 +1987,15 @@ void mtxf_mul_vec3s(Mat4 mtx, Vec3s b) {
     b[2] = x * mtx[0][2] + y * mtx[1][2] + z * mtx[2][2] + mtx[3][2];
 }
 
+void mtxf_mul_vec3f(Mat4 mtx, Vec3f b) {
+    register f32 x = b[0];
+    register f32 y = b[1];
+    register f32 z = b[2];
+
+    b[0] = x * mtx[0][0] + y * mtx[1][0] + z * mtx[2][0] + mtx[3][0];
+    b[1] = x * mtx[0][1] + y * mtx[1][1] + z * mtx[2][1] + mtx[3][1];
+    b[2] = x * mtx[0][2] + y * mtx[1][2] + z * mtx[2][2] + mtx[3][2];
+} 
 /**
  * Convert float matrix 'src' to fixed point matrix 'dest'.
  * The float matrix may not contain entries larger than 65536 or the console

src/engine/math_util.h

@@ -59,6 +59,7 @@ void mtxf_align_terrain_triangle(Mat4 mtx, Vec3f pos, s16 yaw, f32 radius);
 void mtxf_mul(Mat4 dest, Mat4 a, Mat4 b);
 void mtxf_scale_vec3f(Mat4 dest, Mat4 mtx, Vec3f s);
 void mtxf_mul_vec3s(Mat4 mtx, Vec3s b);
+void mtxf_mul_vec3f(Mat4 mtx, Vec3f b);
 void mtxf_to_mtx(Mtx *dest, Mat4 src);
 void mtxf_rotate_xy(Mtx *mtx, s16 angle);
 void get_pos_from_transform_mtx(Vec3f dest, Mat4 objMtx, Mat4 camMtx);

src/game/platform_displacement.c

@@ -0,0 +1,177 @@
+#include "../engine/math_util.h"
+#include "../engine/surface_collision.h"
+#include "level_update.h"
+#include "../include/object_fields.h"
+#include "object_stuff.h"
+#include "platform_displacement.h"
+#include "../shim.h"
+
+struct SurfaceObjectTransform *gMarioPlatform = NULL;
+
+/**
+ * Determine if Mario is standing on a platform object, meaning that he is
+ * within 4 units of the floor. Set his referenced platform object accordingly.
+ */
+void update_mario_platform(void) {
+    struct Surface *floor;
+    UNUSED u32 unused;
+    f32 marioX;
+    f32 marioY;
+    f32 marioZ;
+    f32 floorHeight;
+    u32 awayFromFloor;
+
+    if (gMarioObject == NULL) {
+        return;
+    }
+
+    //! If Mario moves onto a rotating platform in a PU, the find_floor call
+    //  will detect the platform and he will end up receiving a large amount
+    //  of displacement since he is considered to be far from the platform's
+    //  axis of rotation.
+
+    marioX = gMarioObject->oPosX;
+    marioY = gMarioObject->oPosY;
+    marioZ = gMarioObject->oPosZ;
+    floorHeight = find_floor(marioX, marioY, marioZ, &floor);
+
+    if (absf(marioY - floorHeight) < 4.0f) {
+        awayFromFloor = 0;
+    } else {
+        awayFromFloor = 1;
+    }
+
+    switch (awayFromFloor) {
+        case 1:
+            gMarioPlatform = NULL;
+            gMarioObject->platform = NULL;
+            break;
+
+        case 0:
+            if (floor != NULL && floor->object != NULL) {
+                gMarioPlatform = (struct SurfaceObjectTransform *)floor->object;
+                gMarioObject->platform = floor->object;
+            } else {
+                gMarioPlatform = NULL;
+                gMarioObject->platform = NULL;
+            }
+            break;
+    }
+}
+
+/**
+ * Get Mario's position and store it in x, y, and z.
+ */
+static void get_mario_pos(f32 *x, f32 *y, f32 *z) {
+    *x = gMarioState->pos[0];
+    *y = gMarioState->pos[1];
+    *z = gMarioState->pos[2];
+}
+
+/**
+ * Set Mario's position.
+ */
+static void set_mario_pos(f32 x, f32 y, f32 z) {
+    gMarioState->pos[0] = x;
+    gMarioState->pos[1] = y;
+    gMarioState->pos[2] = z;
+}
+
+/**
+ * Apply one frame of platform rotation to Mario or an object using the given
+ * platform. If isMario is false, use gCurrentObject.
+ */
+void apply_platform_displacement(u32 isMario, struct SurfaceObjectTransform *platform) {
+    f32 x;
+    f32 y;
+    f32 z;
+    f32 platformPosX;
+    f32 platformPosY;
+    f32 platformPosZ;
+    Vec3f currentObjectOffset;
+    Vec3f relativeOffset;
+    Vec3f newObjectOffset;
+    Vec3s rotation;
+    UNUSED s16 unused1;
+    UNUSED s16 unused2;
+    UNUSED s16 unused3;
+    f32 displaceMatrix[4][4];
+
+    rotation[0] = platform->aAngleVelPitch;
+    rotation[1] = platform->aAngleVelYaw;
+    rotation[2] = platform->aAngleVelRoll;
+
+//  if (isMario) {
+//      D_8032FEC0 = 0;
+        get_mario_pos(&x, &y, &z);
+//  } else {
+//      x = gCurrentObject->aPosX;
+//      y = gCurrentObject->aPosY;
+//      z = gCurrentObject->aPosZ;
+//  }
+
+    x += platform->aVelX;
+    z += platform->aVelZ;
+
+    if (rotation[0] != 0 || rotation[1] != 0 || rotation[2] != 0) {
+        unused1 = rotation[0];
+        unused2 = rotation[2];
+        unused3 = platform->aFaceAngleYaw;
+
+        if (isMario) {
+            gMarioState->faceAngle[1] += rotation[1];
+        }
+
+        platformPosX = platform->aPosX;
+        platformPosY = platform->aPosY;
+        platformPosZ = platform->aPosZ;
+
+        currentObjectOffset[0] = x - platformPosX;
+        currentObjectOffset[1] = y - platformPosY;
+        currentObjectOffset[2] = z - platformPosZ;
+
+        rotation[0] = platform->aFaceAnglePitch - platform->aAngleVelPitch;
+        rotation[1] = platform->aFaceAngleYaw - platform->aAngleVelYaw;
+        rotation[2] = platform->aFaceAngleRoll - platform->aAngleVelRoll;
+
+        mtxf_rotate_zxy_and_translate(displaceMatrix, currentObjectOffset, rotation);
+        linear_mtxf_transpose_mul_vec3f(displaceMatrix, relativeOffset, currentObjectOffset);
+
+        rotation[0] = platform->aFaceAnglePitch;
+        rotation[1] = platform->aFaceAngleYaw;
+        rotation[2] = platform->aFaceAngleRoll;
+
+        mtxf_rotate_zxy_and_translate(displaceMatrix, currentObjectOffset, rotation);
+        linear_mtxf_mul_vec3f(displaceMatrix, newObjectOffset, relativeOffset);
+
+        x = platformPosX + newObjectOffset[0];
+        y = platformPosY + newObjectOffset[1];
+        z = platformPosZ + newObjectOffset[2];
+    }
+
+//  if (isMario) {
+        set_mario_pos(x, y, z);
+//  } else {
+//      gCurrentObject->oPosX = x;
+//      gCurrentObject->oPosY = y;
+//      gCurrentObject->oPosZ = z;
+//  }
+}
+
+/**
+ * If Mario's platform is not null, apply platform displacement.
+ */
+void apply_mario_platform_displacement(void) {
+    struct SurfaceObjectTransform *platform = gMarioPlatform;
+
+    if (gMarioObject != NULL && platform != NULL) {
+        apply_platform_displacement(TRUE, platform);
+    }
+}
+
+/**
+ * Set Mario's platform to NULL.
+ */
+void clear_mario_platform(void) {
+    gMarioPlatform = NULL;
+}

src/game/platform_displacement.h

@@ -0,0 +1,11 @@
+#ifndef PLATFORM_DISPLACEMENT_H
+#define PLATFORM_DISPLACEMENT_H
+
+#include "../include/PR/ultratypes.h"
+#include "../include/types.h"
+
+void update_mario_platform(void);
+void apply_mario_platform_displacement(void);
+void clear_mario_platform(void);
+
+#endif // PLATFORM_DISPLACEMENT_H

src/gfx_adapter.c

@@ -23,7 +23,7 @@ static float *s_colorPtr;
 static float *s_normalPtr;
 static float *s_uvPtr;
 
-static void mtxf_mul_vec3f(Mat4 mtx, Vec3f b, float w, Vec3f out)
+static void mtxf_mul_vec3f_x(Mat4 mtx, Vec3f b, float w, Vec3f out)
 {
     out[0] = b[0] * mtx[0][0] + b[1] * mtx[1][0] + b[2] * mtx[2][0] + w * mtx[3][0];
     out[1] = b[0] * mtx[0][1] + b[1] * mtx[1][1] + b[2] * mtx[2][1] + w * mtx[3][1];
@@ -78,21 +78,21 @@ static void process_display_list( void *dl )
                 Vec3f n1 = { ((float)nx1) / 128.0f, ((float)ny1) / 128.0f, ((float)nz1) / 128.0f };
                 Vec3f n2 = { ((float)nx2) / 128.0f, ((float)ny2) / 128.0f, ((float)nz2) / 128.0f };
 
-                mtxf_mul_vec3f( s_curMatrix, p0, 1.0f, s_trianglePtr );
+                mtxf_mul_vec3f_x( s_curMatrix, p0, 1.0f, s_trianglePtr );
                 s_trianglePtr += 3;
-                mtxf_mul_vec3f( s_curMatrix, p1, 1.0f, s_trianglePtr );
+                mtxf_mul_vec3f_x( s_curMatrix, p1, 1.0f, s_trianglePtr );
                 s_trianglePtr += 3;
-                mtxf_mul_vec3f( s_curMatrix, p2, 1.0f, s_trianglePtr );
+                mtxf_mul_vec3f_x( s_curMatrix, p2, 1.0f, s_trianglePtr );
                 s_trianglePtr += 3;
 
-                // TODO normals arent correct under non-uniform scale. multiple by inverse/transpose
+                // TODO normals arent correct under non-uniform scale. multiply by inverse/transpose
-                mtxf_mul_vec3f( s_curMatrix, n0, 0.0f, s_normalPtr );
+                mtxf_mul_vec3f_x( s_curMatrix, n0, 0.0f, s_normalPtr );
                 vec3f_normalize( s_normalPtr );
                 s_normalPtr += 3;
-                mtxf_mul_vec3f( s_curMatrix, n1, 0.0f, s_normalPtr );
+                mtxf_mul_vec3f_x( s_curMatrix, n1, 0.0f, s_normalPtr );
                 vec3f_normalize( s_normalPtr );
                 s_normalPtr += 3;
-                mtxf_mul_vec3f( s_curMatrix, n2, 0.0f, s_normalPtr );
+                mtxf_mul_vec3f_x( s_curMatrix, n2, 0.0f, s_normalPtr );
                 vec3f_normalize( s_normalPtr );
                 s_normalPtr += 3;
 

src/libsm64.c

@@ -21,6 +21,7 @@
 #include "gfx_adapter.h"
 #include "load_anim_data.h"
 #include "load_tex_data.h"
+#include "game/platform_displacement.h"
 
 static struct AllocOnlyPool *s_mario_geo_pool;
 static struct GraphNode *s_mario_graph_node;
@@ -126,6 +127,11 @@ void sm64_mario_reset( int16_t marioX, int16_t marioY, int16_t marioZ )
     find_floor( marioX, marioY, marioZ, &gMarioState->floor );
 }
 
+static void update_terrain_objects( void )
+{
+    update_dynamic_surface_list(); 
+}
+
 static void update_non_terrain_objects( void )
 {
     bhv_mario_update();
@@ -134,11 +140,11 @@ static void update_non_terrain_objects( void )
 static void update_objects( void )
 {
     //clear_dynamic_surfaces();
-    //update_terrain_objects();
+    update_terrain_objects();
-    //apply_mario_platform_displacement();
+    apply_mario_platform_displacement();
     //detect_object_collisions();
     update_non_terrain_objects();
-    //update_mario_platform();
+    update_mario_platform();
 }
 
 void sm64_mario_tick( const struct SM64MarioInputs *inputs, struct SM64MarioState *outState, struct SM64MarioGeometryBuffers *outBuffers )
@@ -169,7 +175,9 @@ void sm64_mario_tick( const struct SM64MarioInputs *inputs, struct SM64MarioStat
 
 void sm64_global_terminate( void )
 {
-    // TODO free
+    // TODO free other things
+
+    surfaces_unload_all();
 }
 
 uint32_t sm64_load_surface_object( const struct SM64SurfaceObject *surfaceObject )

src/load_surfaces.c

@@ -5,17 +5,60 @@
 
 #include "include/types.h"
 #include "include/surface_terrains.h"
+#include "engine/math_util.h"
 #include "shim.h"
 
+struct LoadedSurfaceObject
+{
+    struct SurfaceObjectTransform transform;
+    uint32_t surfaceCount;
+    struct SM64Surface *surfaces;
+};
+
 static uint32_t s_static_surface_count = 0;
 static struct Surface *s_static_surface_list = NULL;
 
 static uint32_t s_surface_object_count = 0;
-static struct SM64SurfaceObject *s_surface_object_list = NULL;
+static struct LoadedSurfaceObject *s_surface_object_list = NULL;
 
 static uint32_t s_dynamic_surface_count = 0;
 static struct Surface *s_dynamic_surface_list = NULL;
 
+
+static void init_transform( struct SurfaceObjectTransform *out, const struct SM64ObjectTransform *in )
+{
+    out->aVelX = 0.0f;
+    out->aVelY = 0.0f;
+    out->aVelZ = 0.0f;
+    out->aPosX = in->position[0];
+    out->aPosY = in->position[1];
+    out->aPosZ = in->position[2];
+
+    out->aAngleVelPitch = 0.0f;
+    out->aAngleVelYaw   = 0.0f;
+    out->aAngleVelRoll  = 0.0f;
+    out->aFaceAnglePitch = in->eulerRotation[0];
+    out->aFaceAngleYaw   = in->eulerRotation[1];
+    out->aFaceAngleRoll  = in->eulerRotation[2];
+}
+
+static void update_transform( struct SurfaceObjectTransform *out, const struct SM64ObjectTransform *in )
+{
+    out->aVelX = in->position[0] - out->aPosX;
+    out->aVelY = in->position[1] - out->aPosY;
+    out->aVelZ = in->position[2] - out->aPosZ;
+    out->aPosX = in->position[0];
+    out->aPosY = in->position[1];
+    out->aPosZ = in->position[2];
+
+    out->aFaceAnglePitch = in->eulerRotation[0] - out->aFaceAnglePitch;
+    out->aFaceAngleYaw   = in->eulerRotation[1] - out->aFaceAngleYaw;
+    out->aFaceAngleRoll  = in->eulerRotation[2] - out->aFaceAngleRoll;
+    out->aFaceAnglePitch = in->eulerRotation[0];
+    out->aFaceAngleYaw   = in->eulerRotation[1];
+    out->aFaceAngleRoll  = in->eulerRotation[2];
+}
+
 /**
  * Returns whether a surface has exertion/moves Mario
  * based on the surface type.
@@ -40,7 +83,7 @@ static s32 surface_has_force(s16 surfaceType) {
     return hasForce;
 }
 
-static void engine_surface_from_lib_surface( struct Surface *surface, const struct SM64Surface *libSurf, const struct SM64ObjectTransform *transform )
+static void engine_surface_from_lib_surface( struct Surface *surface, const struct SM64Surface *libSurf, const struct SurfaceObjectTransform *transform )
 {
     int16_t type = libSurf->type;
     int16_t force = libSurf->force;
@@ -58,6 +101,30 @@ static void engine_surface_from_lib_surface( struct Surface *surface, const stru
     f32 nx, ny, nz;
     f32 mag;
 
+    if( transform != NULL )
+    {
+        Mat4 m;
+        Vec3s rotation = {
+            (short)( -transform->aFaceAnglePitch / 180.0f * 32768.0f ),
+            (short)( -transform->aFaceAngleYaw   / 180.0f * 32768.0f ),
+            (short)( -transform->aFaceAngleRoll  / 180.0f * 32768.0f )
+        };
+        Vec3f position = { transform->aPosX, transform->aPosY, transform->aPosZ };
+        mtxf_rotate_zxy_and_translate(m, position, rotation);
+
+        Vec3f v1 = { x1, y1, z1 };
+        Vec3f v2 = { x2, y2, z2 };
+        Vec3f v3 = { x3, y3, z3 };
+
+        mtxf_mul_vec3f( m, v1 );
+        mtxf_mul_vec3f( m, v2 );
+        mtxf_mul_vec3f( m, v3 );
+
+        x1 = v1[0]; y1 = v1[1]; z1 = v1[2];
+        x2 = v2[0]; y2 = v2[1]; z2 = v2[2];
+        x3 = v3[0]; y3 = v3[1]; z3 = v3[2];
+    }
+
     // (v2 - v1) x (v3 - v2)
     nx = (y2 - y1) * (z3 - z2) - (z2 - z1) * (y3 - y2);
     ny = (z2 - z1) * (x3 - x2) - (x2 - x1) * (z3 - z2);
@@ -89,8 +156,6 @@ static void engine_surface_from_lib_surface( struct Surface *surface, const stru
     ny *= mag;
     nz *= mag;
 
-    // TODO apply tranform
-
     surface->vertex1[0] = x1;
     surface->vertex2[0] = x2;
     surface->vertex3[0] = x3;
@@ -128,11 +193,9 @@ static void engine_surface_from_lib_surface( struct Surface *surface, const stru
     return surface;
 }
 
-static void update_dynamic_surface_list( void )
+void update_dynamic_surface_list( void )
 {
-    if( s_dynamic_surface_list != NULL )
     free( s_dynamic_surface_list );
-
     s_dynamic_surface_count = 0;
     s_dynamic_surface_list = NULL;
 
@@ -174,15 +237,35 @@ uint32_t surfaces_load_object( const struct SM64SurfaceObject *surfaceObject )
 {
     uint32_t idx = s_surface_object_count;
     s_surface_object_count++;
-    s_surface_object_list = realloc( s_surface_object_list, s_surface_object_count * sizeof( struct SM64SurfaceObject ));
+    s_surface_object_list = realloc( s_surface_object_list, s_surface_object_count * sizeof( struct LoadedSurfaceObject ));
 
-    struct SM64SurfaceObject *obj = &s_surface_object_list[idx];
+    struct LoadedSurfaceObject *obj = &s_surface_object_list[idx];
-    obj->transform = surfaceObject->transform;
+    init_transform( &obj->transform, &surfaceObject->transform );
     obj->surfaceCount = surfaceObject->surfaceCount;
     obj->surfaces = malloc( obj->surfaceCount * sizeof( struct SM64Surface ));
     memcpy( obj->surfaces, surfaceObject->surfaces, obj->surfaceCount * sizeof( struct SM64Surface ));
 
-    update_dynamic_surface_list();
-
     return idx;
 }
+
+void surface_object_update_transform( uint32_t objId, const struct SM64ObjectTransform *newTransform )
+{
+    update_transform( &s_surface_object_list[objId].transform, newTransform );
+}
+
+void surfaces_unload_all( void )
+{
+    free( s_static_surface_list );
+    s_static_surface_count = 0;
+    s_static_surface_list = NULL;
+
+    for( int i = 0; i < s_surface_object_count; ++i )
+        free( s_surface_object_list[i].surfaces );
+    free( s_surface_object_list );
+    s_surface_object_count = 0;
+    s_surface_object_list = NULL;
+
+    free( s_dynamic_surface_list );
+    s_dynamic_surface_count = 0;
+    s_dynamic_surface_list = NULL;
+}

src/load_surfaces.h

@@ -5,5 +5,8 @@
 extern struct Surface *loaded_surface_get_at_index( uint32_t index );
 extern uint32_t loaded_surface_get_count();
 
+extern void update_dynamic_surface_list( void );
 extern void surfaces_load_static_libsm64( const struct SM64Surface *surfaceArray, uint32_t numSurfaces );
 extern uint32_t surfaces_load_object( const struct SM64SurfaceObject *surfaceObject );
+extern void surface_object_update_transform( uint32_t objId, const struct SM64ObjectTransform *newTransform );
+extern void surfaces_unload_all( void );

src/shim.h

@@ -22,6 +22,20 @@
 #define disable_time_stop() ({})
 #define play_cutscene_music(a) ({})
 
+struct SurfaceObjectTransform
+{
+    float aPosX, aPosY, aPosZ;
+    float aVelX, aVelY, aVelZ;
+
+    s16 aFaceAnglePitch;
+    s16 aFaceAngleYaw;
+    s16 aFaceAngleRoll;
+
+    s16 aAngleVelPitch;
+    s16 aAngleVelYaw;
+    s16 aAngleVelRoll;
+};
+
 struct SurfaceNode
 {
     struct SurfaceNode *next;