From 47b158cce9663665b770fe08d83782b7d40a42bf Mon Sep 17 00:00:00 2001
From: MysterD <myster@d>
Date: Wed, 14 Oct 2020 23:18:05 -0700
Subject: [PATCH] Bubbled visual improvements (pitch to player)

---
 src/game/behaviors/water_objs.inc.c | 17 +++-------
 src/game/mario_actions_automatic.c  | 50 +++++++++++++++++++++++++++--
 src/game/object_helpers.c           | 12 +++++++
 src/game/object_helpers.h           |  1 +
 4 files changed, 65 insertions(+), 15 deletions(-)

diff --git a/src/game/behaviors/water_objs.inc.c b/src/game/behaviors/water_objs.inc.c
index 5701bfb9..c8b48916 100644
--- a/src/game/behaviors/water_objs.inc.c
+++ b/src/game/behaviors/water_objs.inc.c
@@ -73,19 +73,10 @@ void bhv_small_water_wave_loop(void) {
 void bhv_bubble_player_loop(void) {
     struct MarioState* marioState = &gMarioStates[o->heldByPlayerIndex];
 
-    // grab positions to find the mid-point
-    f32* torsoPos = marioState->marioBodyState->torsoPos;
-    f32* pos = marioState->pos;
-
-    // sanity check torsoPos
-    if (marioState->marioObj->header.gfx.node.flags & GRAPH_RENDER_INVISIBLE) {
-        torsoPos = marioState->pos;
-    }
-
-    // set the position + offset
-    o->oPosX = (torsoPos[0] + pos[0]) / 2;
-    o->oPosY = (torsoPos[1] + pos[1]) / 2 + 30.0f;
-    o->oPosZ = (torsoPos[2] + pos[2]) / 2;
+    // set position
+    o->oPosX = marioState->pos[0];
+    o->oPosY = marioState->pos[1] + 35;
+    o->oPosZ = marioState->pos[2];
 
     // slowly rotate the bubble
     o->oFaceAnglePitch += 300;
diff --git a/src/game/mario_actions_automatic.c b/src/game/mario_actions_automatic.c
index 45105614..be3a824f 100644
--- a/src/game/mario_actions_automatic.c
+++ b/src/game/mario_actions_automatic.c
@@ -865,10 +865,49 @@ s32 act_tornado_twirling(struct MarioState *m) {
     return FALSE;
 }
 
+
+static void bubbled_offset_visual(struct MarioState* m) {
+    // scary 3d trig ahead
+
+    f32 forwardOffset = 25;
+    f32 upOffset = -35;
+
+    // figure out forward vector
+    Vec3f forward = {
+        sins(m->faceAngle[1]) * coss(m->faceAngle[0]),
+        -sins(m->faceAngle[0]),
+        coss(m->faceAngle[1]) * coss(m->faceAngle[0]),
+    };
+    vec3f_normalize(forward);
+
+    // figure out right vector
+    Vec3f globalUp = { 0, 1, 0 };
+    Vec3f right = { 0 };
+    vec3f_cross(right, forward, globalUp);
+    vec3f_normalize(right);
+
+    // figure out up vector
+    Vec3f up = { 0 };
+    vec3f_cross(up, right, forward);
+    vec3f_normalize(up);
+
+    // offset forward direction
+    vec3f_mul(forward, forwardOffset);
+    vec3f_add(m->marioObj->header.gfx.pos, forward);
+
+    // offset up direction
+    vec3f_mul(up, upOffset);
+    vec3f_add(m->marioObj->header.gfx.pos, up);
+
+    // offset global up direction
+    m->marioObj->header.gfx.pos[1] -= upOffset;
+}
+
 s32 act_bubbled(struct MarioState* m) {
     struct MarioState* targetMarioState = nearest_mario_state_to_object(m->marioObj);
     struct Object* target = targetMarioState->marioObj;
     int angleToPlayer = obj_angle_to_object(m->marioObj, target);
+    int pitchToPlayer = obj_pitch_to_object(m->marioObj, target);
     int distanceToPlayer = dist_between_objects(m->marioObj, target);
 
     // trigger warp if all are bubbled
@@ -901,6 +940,8 @@ s32 act_bubbled(struct MarioState* m) {
     set_mario_animation(m, MARIO_ANIM_SLEEP_IDLE);
 
     // force inputs
+    f32 oldPitch = m->faceAngle[0];
+    f32 oldYaw   = m->faceAngle[1];
     m->faceAngle[0] = 0;
     m->faceAngle[1] = m->intendedYaw;
     m->forwardVel = m->intendedMag;
@@ -928,8 +969,13 @@ s32 act_bubbled(struct MarioState* m) {
     }
 
     // always look toward target
-    m->faceAngle[1] = angleToPlayer;
-    m->marioObj->header.gfx.angle[1] = angleToPlayer;
+    m->faceAngle[0] = pitchToPlayer - approach_s32((s16)(pitchToPlayer - oldPitch), 0, 0x600, 0x600);
+    m->faceAngle[1] = angleToPlayer - approach_s32((s16)(angleToPlayer - oldYaw  ), 0, 0x600, 0x600);
+    m->marioObj->header.gfx.angle[0] = m->faceAngle[0];
+    m->marioObj->header.gfx.angle[1] = m->faceAngle[1];
+
+    // offset the player model to be in the center of the bubble
+    bubbled_offset_visual(m);
 
     // make invisible on -1 lives
     if (m->playerIndex == 0) {
diff --git a/src/game/object_helpers.c b/src/game/object_helpers.c
index f18d0176..3b9daa1f 100644
--- a/src/game/object_helpers.c
+++ b/src/game/object_helpers.c
@@ -430,6 +430,18 @@ s16 obj_angle_to_object(struct Object *obj1, struct Object *obj2) {
     return angle;
 }
 
+s16 obj_pitch_to_object(struct Object* obj, struct Object* target) {
+    f32 a, b, c, d;
+    a = target->oPosX - obj->oPosX;
+    c = target->oPosZ - obj->oPosZ;
+    a = sqrtf(a * a + c * c);
+
+    b = -obj->oPosY;
+    d = -target->oPosY;
+
+    return atan2s(a, d - b);
+}
+
 s16 obj_angle_to_point(struct Object *obj, f32 pointX, f32 pointZ) {
     f32 z1, x1, z2, x2;
     s16 angle;
diff --git a/src/game/object_helpers.h b/src/game/object_helpers.h
index 7a64c1ee..477871de 100644
--- a/src/game/object_helpers.h
+++ b/src/game/object_helpers.h
@@ -92,6 +92,7 @@ f32 approach_f32_symmetric(f32 value, f32 target, f32 increment);
 s16 approach_s16_symmetric(s16 value, s16 target, s16 increment);
 s32 cur_obj_rotate_yaw_toward(s16 target, s16 increment);
 s16 obj_angle_to_object(struct Object *obj1, struct Object *obj2);
+s16 obj_pitch_to_object(struct Object* obj, struct Object* target);
 s16 obj_angle_to_point(struct Object *obj, f32 pointX, f32 pointZ);
 s16 obj_turn_toward_object(struct Object *obj, struct Object *target, s16 angleIndex, s16 turnAmount);
 void obj_set_parent_relative_pos(struct Object *obj, s16 relX, s16 relY, s16 relZ);