Got a point rendering, but still need to cull it if behind

Makefile

@@ -1,6 +1,6 @@
 CC = gcc
 CFLAGS = -g
-LDLIBS = -lraylib
+LDLIBS = -lraylib -lm
 
 objects  = c3d.o
 

c3d.c

@@ -1,19 +1,168 @@
+#include <math.h>
 #include <stdio.h>
 #include "raylib.h"
 
 const int RENDERWIDTH = 1920;
 const int RENDERHEIGHT = 1080;
 
+const int HALFWIDTH = RENDERWIDTH / 2;
+const int HALFHEIGHT = RENDERHEIGHT / 2;
+
+
 const int SCREENWIDTH = 1280;
 const int SCREENHEIGHT = 720;
 
 const float WIDTHSCALE = (float)RENDERWIDTH / SCREENWIDTH;
 const float HEIGHTSCALE = (float) RENDERHEIGHT / SCREENHEIGHT;
 
+
+const float fov = 90.0 / 180.0 * 3.14159265369;
+const float half_fov = fov / 2;
+
+float proj; //HALFWIDTH / tan(half_fov) Inited later
+
+
+/*
+  The point is now rendering, but it is in front of and behind us at the same time
+*/
+
+
+struct LocalCam {
+    Vector3 position;
+    Vector3 velocity;
+    Vector3 acceleration;
+    Vector3 angles;
+    Vector3 angleVelocity;
+    Vector3 angleAcceleration;
+};
+
+typedef struct LocalCam LocalCam;
+
+Vector3 Vector3Sum(Vector3 v1, Vector3 v2) {
+    Vector3 retvec;
+    retvec.x = v1.x + v2.x;
+    retvec.y = v1.y + v2.y;
+    retvec.z = v1.z + v2.z;
+    return retvec;
+}
+
+Vector3 Vector3Scale(Vector3 v1, float scale) {
+    Vector3 retvec;
+    retvec.x = v1.x * scale;
+    retvec.y = v1.y * scale;
+    retvec.z = v1.z * scale;
+    return retvec;
+}
+
+Vector3 RotateAboutX(Vector3 v, double radians){
+    Vector3 rotatedvector;
+    rotatedvector.x = 1*v.x + (0) + (0);
+    rotatedvector.y = (0) + cos(radians)*v.y + (-sin(radians)*v.z);
+    rotatedvector.z = 0 + sin(radians)*v.y + cos(radians)*v.z;
+    return rotatedvector;
+}
+Vector3 RotateAboutY(Vector3 v, double radians){
+    Vector3 rotatedvector;
+    rotatedvector.x = cos(radians)*v.x + (0) + sin(radians)*v.z;
+    rotatedvector.y = (0) + 1*v.y + (0);
+    rotatedvector.z = -sin(radians)*v.x + 0 + cos(radians)*v.z;
+    return rotatedvector;
+}
+
+Vector3 RotateAboutZ(Vector3 v, double radians){
+    Vector3 rotatedvector;
+    rotatedvector.x = cos(radians)*v.x + (-sin(radians)*v.y) + (0);
+    rotatedvector.y = sin(radians)*v.x + cos(radians)*v.y + (0);
+    rotatedvector.z = 0 + 0 + 1*v.z;
+    return rotatedvector;
+}
+
+void CtrlLocalCam(LocalCam *cam, float time) {
+    cam->velocity.x = 0;
+    cam->velocity.y = 0;
+    cam->velocity.z = 0;
+    cam->angleVelocity.x = 0;
+    cam->angleVelocity.y = 0;
+    cam->angleVelocity.z = 0;
+
+    if (IsKeyDown(KEY_W)) {
+	Vector3 forceForward = (Vector3){0,0,500};
+	Vector3 rotatedforce = RotateAboutY(forceForward,cam->angles.y);
+	cam->velocity = Vector3Sum(cam->velocity,rotatedforce);
+
+	//cam->velocity.z = cam->velocity.z + 50000*time*sin(cam->angles.y);
+	//cam->velocity.x = cam->velocity.x + 500*time*cos(cam->angles.y);
+    }
+
+    if(IsKeyDown(KEY_SPACE)) {
+	cam->velocity.y = cam->velocity.y - 50;
+    }
+
+    if (IsKeyDown(KEY_Q)){
+	cam->angleVelocity.y = cam->angleVelocity.y + time*1000;
+    }
+    if (IsKeyDown(KEY_E)){
+	cam->angleVelocity.y = cam->angleVelocity.y - time*1000;
+    }
+
+    if (IsKeyDown(KEY_G)) {
+	printf("PosX: %f PosY: %f, PosZ: %f\n",cam->position.x,cam->position.y,cam->position.z);
+    }
+}
+
+void LocalCamApplyVelo(LocalCam *cam,float time){
+    cam->position.x = cam->position.x + cam->velocity.x * time;
+    cam->position.y = cam->position.y + cam->velocity.y * time;
+    cam->position.z = cam->position.z + cam->velocity.z * time;
+
+    cam->angles.x = cam->angles.x + cam->angleVelocity.x * time;
+    cam->angles.y = cam->angles.y + cam->angleVelocity.y * time;
+    cam->angles.z = cam->angles.z + cam->angleVelocity.z * time;
+    //printf("%f %f %f\n",cam->angles.x, cam->angles.y, cam->angles.z);
+}
+
+
+
+
+
+Vector2 Conv3Dto2D(Vector3 v) {
+    Vector2 returnvector;
+    returnvector.x = proj * v.x / v.z;
+    returnvector.y = proj * v.y / v.z;
+    return returnvector;
+}
+
+Vector2 Conv2DCenteredToScreen(Vector2 v) {
+    Vector2 returnvector;
+    returnvector.x = v.x + HALFWIDTH;
+    returnvector.y = v.y + HALFHEIGHT;
+    return returnvector;
+}
+
+Vector3 TransformWithCam(Vector3 v, LocalCam * cam) {
+    Vector3 returnvector;
+    returnvector.x = v.x - cam->position.x;
+    returnvector.y = v.y - cam->position.y;
+    returnvector.z = v.z - cam->position.z;
+
+    returnvector = RotateAboutZ(returnvector,cam->angles.z);
+    returnvector = RotateAboutY(returnvector,cam->angles.y);
+    returnvector = RotateAboutZ(returnvector,cam->angles.z);
+
+    //printf("Before: %f %f %f, After: %f %f %f\n", v.x, v.y, v.z, returnvector.x, returnvector.y, returnvector.z);
+    return returnvector;
+}
+
 int main() {
+    proj = HALFWIDTH / tan(half_fov);
+
+
+    
+    
     SetConfigFlags(FLAG_WINDOW_UNDECORATED);
     InitWindow(SCREENWIDTH, SCREENHEIGHT, "raylib [core] example - basic window");
     
+
     // SetWindowPosition(0,1080);
     Vector2 a = GetMonitorPosition(0);
     int mh = GetMonitorHeight(0);
@@ -26,11 +175,33 @@ int main() {
     RenderTexture2D renderTexture = LoadRenderTexture(RENDERWIDTH,RENDERHEIGHT);
 
 
+    // Init cube model
+    // TODO: Load from obj
+
+    LocalCam camera;
+    camera.position = (Vector3){0,0,0};
+    camera.acceleration = (Vector3){0,0,0};
+    camera.angleAcceleration = (Vector3){0,0,0};
+    camera.angles = (Vector3){0,0,0};
+    camera.velocity = (Vector3){0,0,0};
+    camera.angleVelocity = (Vector3){0,0,0};
+    
+    Vector3 point = (Vector3){0,0,10};
+
+    
+    
     while (!WindowShouldClose()) {
+	float frametime = GetFrameTime();
+	CtrlLocalCam(&camera,frametime);
+	LocalCamApplyVelo(&camera,frametime);
         BeginTextureMode(renderTexture);
         ClearBackground(WHITE);
         DrawText("Congrats! You created your first window!", 190, 200, 20,
                  BLACK);
+	Vector3 TransVector = TransformWithCam(point,&camera);
+	Vector2 MPos = Conv3Dto2D(TransVector);
+	Vector2 FinPos = Conv2DCenteredToScreen(MPos);
+	DrawCircleV(FinPos,100,BLACK);
         EndTextureMode();
 
 	//Copytexture to main display :0