3DS Physics Sandbox

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <3ds.h>
#include <sf2d.h>
//#include "physobject.cpp";

#define CONFIG_3D_SLIDERSTATE (*(float *)0x1FF81080)
#define debug

#define rw 50
#define rh 50
#define count 10

//forward declarations
char* get_mod_val_id(u8);

float gravity_y = -10.0f;
float gravity_x = 0.0f;

int sel = 0;

char* f[10] = { "0", "1", "2", "3", "4", "5", "6", "7", "8", "9" };

bool global = true;

class physobject {
public:
    float rect_x = 320 / 2;
    float rect_y = 240 / 2;
    u8 mov_rect = 0.0f;
    float vel_x = 0.0f;
    float vel_y = 0.0f;
    float elasticity = .5f;
    float friction = .5f;
    int kind = 0; //0 = rectangle 1 = circle 2 = triangle
    u32 color;
};

int main()
{
    // Set the random seed based on the time
    srand(time(NULL));

    //init screen background to white
    sf2d_init();
    sf2d_set_clear_color(RGBA8(0xFF, 0xFF, 0xFF, 0xFF));
    sf2d_set_3D(0);
    sf2d_set_vblank_wait(1);

    physobject arr[count];

    for (size_t i = 0; i < count; i++)
    {
        arr[i].rect_x = rand() % 320;
        arr[i].rect_y = rand() % 240;
        arr[i].color = RGBA8(rand() % 255, rand() % 255, rand() % 255, 255);
        arr[i].kind = rand() % 3;
    }

    //Variables

    //Represents the current modifiable value: gravity_y, gravity_x, friction, elasticity
    u8 mod_val = 0;

    //coordinates of screen touch
    u16 touch_x = 320/2;
    u16 touch_y = 240/2;

    //increment - determines how much is added or subtracted to mod_val - is always power of 10
    double inc = 1.0f;

    //boolean to determine if touch screen is currently dictating the square's location
    u8 mov_rect = 0.0f;

    //records where the user tapped on the square e.g. if the top right corner is tapped, it will stay at top right corner
    s8 offset_x = 0.0f;
    s8 offset_y = 0.0f;

    //HID variables
    touchPosition touch;
    circlePosition circle;
    u32 held;
    u32 pressed;
    u32 released;

    //physobject objects;

    consoleInit(GFX_TOP, NULL);

    printf("\x1b[0;0HPhysics Sandbox v0.1 by PieFace");
    printf("\x1b[23;0HControls:");
    printf("\x1b[24;0HTap and drag square to move it");
    printf("\x1b[25;0HX - Reset Square");
    printf("\x1b[26;0HDPad L&R - Change between mod_vals");
    printf("\x1b[27;0HDPad U&D - Increment and decrement mod_val");
    printf("\x1b[28;0HL&R - Change increment (pow of 10)");
    printf("\x1b[29;0HStart - Exit");
    while (aptMainLoop()) {

        //gather inputs
        hidScanInput();
        hidCircleRead(&circle);
        held = hidKeysHeld();
        pressed = hidKeysDown();
        released = hidKeysUp();


        //check button presses

        //Press Start to Exit
        if (held & KEY_START)
        {
            break;
        }

        //Read touch values if the screen is pressed
        if (held & KEY_TOUCH)
        {
            hidTouchRead(&touch);
            touch_x = touch.px;
            touch_y = touch.py;
            if(mov_rect)
            {
                arr[sel].vel_x = (touch_x + offset_x - arr[sel].rect_x)*60.0f;
                arr[sel].vel_y = (touch_y + offset_y - arr[sel].rect_y)*60.0f;
            }
        }

        //DPAD up - Increment current mod_val
        if(pressed & KEY_DUP)
        {
            if(mod_val == 0)
            {
                gravity_y += inc;
            }

            if(mod_val == 1)
            {
                gravity_x += inc;
            }

            if(mod_val == 2)
            {
                if (global)
                {
                    for (size_t i = 0; i < count; i++)
                    {
                        arr[i].elasticity += inc;
                        if (arr[i].elasticity > 1) arr[i].elasticity = 1;
                    }
                }
                else
                {
                    arr[sel].elasticity += inc;
                    if (arr[sel].elasticity > 1) arr[sel].elasticity = 1;
                }
            }

            if(mod_val == 3)
            {
                if (global)
                {
                    for (size_t i = 0; i < count; i++)
                    {
                        arr[i].friction += inc;
                        if (arr[i].friction > 1) arr[i].friction = 0;
                    }
                }
                else
                {
                    arr[sel].friction += inc;
                    if (arr[sel].friction > 1) arr[sel].friction = 0;
                }
            }
        }

        //DPAD down - decrement current mod_val
        if(pressed & KEY_DDOWN)
        {
            if(mod_val == 0)
            {
                gravity_y -= inc;
            }

            if(mod_val == 1)
            {
                gravity_x -= inc;
            }

            if(mod_val == 2)
            {
                if (global)
                {
                    for (size_t i = 0; i < count; i++)
                    {
                        arr[i].elasticity -= inc;
                        if (arr[i].elasticity < 0) arr[i].elasticity = 0;
                    }
                }
                else
                {
                    arr[sel].elasticity -= inc;
                    if (arr[sel].elasticity < 0) arr[sel].elasticity = 0;
                }
            }

            if(mod_val == 3)
            {
                if (global)
                {
                    for (size_t i = 0; i < count; i++)
                    {
                        arr[i].friction -= inc;
                        if (arr[i].friction < 0) arr[i].friction = 0;
                    }
                }
                else
                {
                    arr[sel].friction -= inc;
                    if (arr[sel].friction < 0) arr[sel].friction = 0;
                }
            }
        }

        //DPAD Left and Right - Change mod_val
        if(pressed & KEY_DLEFT)
        {
            mod_val--;
            if (mod_val > 3) mod_val = 3;
        }

        if(pressed & KEY_DRIGHT)
        {
            mod_val++;
            if (mod_val > 3) mod_val = 0;
        }


        //L and R - Multiply and divide increment by 10
        if(pressed & KEY_L) inc *= 10;
        if(pressed & KEY_R) inc /= 10;


        //Collisions and Friction
        //Detects collision with edge of the screen
        //Inverts velocity perpendicular to collision and gradually reduces velocity parallel to collision
        for (size_t i = 0; i < count; i++)
        {
            //Collision and friction with top or bottom edge
            if (arr[i].rect_x - rw / 2 <= 1 || arr[i].rect_x + rw / 2 >= 319)
            {
                arr[i].vel_x = -arr[i].vel_x * arr[i].elasticity;

                if (arr[i].rect_x < rw / 2) arr[i].rect_x = rw / 2;
                if (arr[i].rect_x > 320 - rw / 2) arr[i].rect_x = 320 - rw / 2;

                arr[i].vel_y += (arr[i].vel_y > 0 ? -fmin(arr[i].friction, arr[i].vel_y) : -fmax(-arr[i].friction, arr[i].vel_y));
            }

            //Collision and friction with left and right edge
            if (arr[i].rect_y - rh / 2 <= 1 || arr[i].rect_y + rh / 2 >= 239)
            {
                arr[i].vel_y = -arr[i].vel_y * arr[i].elasticity;

                if (arr[i].rect_y < rh / 2) arr[i].rect_y = rh / 2;
                if (arr[i].rect_y > 240 - rh / 2) arr[i].rect_y = 240 - rh / 2;

                arr[i].vel_x += (arr[i].vel_x > 0 ? -fmin(arr[i].friction, arr[i].vel_x) : -fmax(-arr[i].friction, arr[i].vel_x));
            }
        }

        //If the coordinates of the screen tap are inside the square, allow movement of the square
        if (pressed & KEY_TOUCH)
        {
            for (size_t i = 0; i < count; i++)
            {
                if ((touch_x > arr[i].rect_x - rw / 2) && (touch_x < arr[i].rect_x + rw / 2) && (touch_y > arr[i].rect_y - rh / 2) && (touch_y < arr[i].rect_y + rh / 2))
                {
                    sel = i;
                    mov_rect = 1;
                    offset_x = arr[i].rect_x - touch_x;
                    offset_y = arr[i].rect_y - touch_y;
                    break;
                }
            }
        }

        //If the screen is let go, stop allowing screen to control square
        if ((released & KEY_TOUCH))
        {
            mov_rect = 0;
            offset_x = 0;
            offset_y = 0;
        }

        //Press x to reset all values
        if(pressed & KEY_X)
        {
            for (size_t i = 0; i < count; i++)
            {
                arr[i].rect_x = rand() % 320;
                arr[i].rect_y = rand() % 240;
                arr[i].color = RGBA8(rand() % 255, rand() % 255, rand() % 255, 255);
                arr[i].vel_x = 0.0f;
                arr[i].vel_y = 0.0f;
                arr[i].elasticity = .5f;
                arr[i].friction = .5f;
            }
        }

        if (pressed & KEY_Y)
        {
            global = !global;
        }

        //Determines how to move the square
        //If the touch screen is not dictating the square's location, apply gravity and velocity
        //Else move square to where screen is touched
        for (size_t i = 0; i < count; i++)
        {
            if (!mov_rect)
            {
                arr[i].vel_y -= gravity_y;
                arr[i].vel_x += gravity_x;

                arr[i].rect_x += arr[i].vel_x / 60.0f;
                arr[i].rect_y += arr[i].vel_y / 60.0f;
            }
            else if (i == sel)
            {
                arr[sel].rect_x = touch_x + offset_x;
                arr[sel].rect_y = touch_y + offset_y;
            }
        }

        //Live printing of important values
        #ifdef debug
        printf("\x1b[0;41H%s:%5.2f","fps",sf2d_get_fps());
        printf("\x1b[1;0H%s:%03d,%03d","touch_coord",touch_x,touch_y);
        printf("\x1b[2;0H%s:%03.0f,%03.0f","rect_coord", arr[sel].rect_x, arr[sel].rect_y);
        printf("\x1b[3;0H%s:%03d,%03d","offset",offset_x,offset_y);
        printf("\x1b[4;0H%s:%010.3f,%010.3f","velocity", arr[sel].vel_x, -arr[sel].vel_y);
        printf("\x1b[5;0H%s:%f,%f","gravity", gravity_x, gravity_y);
        printf("\x1b[6;0H%s:%f","elasticity", arr[sel].elasticity);
        printf("\x1b[7;0H%s:%f","friction", arr[sel].friction);
        printf("\x1b[8;0H%s:%f","increment", inc);
        printf("\x1b[9;0H%s:%s","mod_val", get_mod_val_id(mod_val));
        printf("\x1b[10;0H%s:%s", "sel", f[sel]);
        printf("\x1b[11;0H%s:%s", "global", global ? "true" : "false");
        #endif

        //draw frame
        sf2d_start_frame(GFX_BOTTOM, GFX_LEFT);

        for (size_t i = count; i-- > 0;)
        {
            if (arr[i].kind == 0)
            {
                sf2d_draw_rectangle((int)arr[i].rect_x - rw / 2, (int)arr[i].rect_y - rh / 2, 50, 50, arr[i].color);
            }
            else if (arr[i].kind == 1)
            {
                sf2d_draw_fill_circle((int)arr[i].rect_x, (int)arr[i].rect_y, rw / 2, arr[i].color);
            }
            else if (arr[i].kind == 2)
            {
                int x = (int)arr[i].rect_x;
                int y = (int)arr[i].rect_y;
                sf2d_draw_triangle(x, y - rh / 2, x - rw / 2, y + rh / 2, x + rw / 2, y + rh / 2, arr[i].color);
            }
        }

        sf2d_end_frame();

        sf2d_swapbuffers();
    }


    sf2d_fini();
    return 0;
}

//returns string of mod_val from id
char* get_mod_val_id(u8 id)
{
    if(id==0) return "gravity_y ";
    if(id==1) return "gravity_x ";
    if(id==2) return "elasticity";
    if(id==3) return "friction  ";
    return "err_bad_id";
}