Skip to content

N
NetWorld
Commit b0598b21 authored by Guillaume Lozenguez's avatar Guillaume Lozenguez
Browse files
Options

merge from dev

parents 8593c4e8 fb1f6986
Show whitespace changes
Inline Side-by-side
Showing with 14204 additions and 69 deletions
.gitignore
View file @ b0598b21
......@@ -2,6 +2,8 @@
build
raylib
doc/html
.vscode/
# Generated files:
......
CMakeLists.txt
View file @ b0598b21
......@@ -11,14 +11,15 @@ set(CMAKE_CXX_FLAGS_RELEASE_INIT "-Wall")
include_directories(${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/src)
# RayLib:
find_package(raylib 2.6 REQUIRED)
#find_package(raylib 3.0 REQUIRED)
#set(raylib_VERBOSE 1)
add_executable(nw-hello src/main-hello.c)
target_link_libraries(nw-hello raylib)
# Local dependency: (RayLib) :
include_directories( ${PROJECT_SOURCE_DIR}/dpd/include )
link_directories( ${PROJECT_SOURCE_DIR}/dpd )
add_executable(nw-viewer src/main-viewer.c src/networld.c)
target_link_libraries(nw-viewer raylib)
add_executable(nw-viewer src/main-viewer.c src/networld.c src/controlpanel.c src/entity.c)
target_link_libraries(nw-viewer raylib pthread dl rt X11 m)
#without cmake package...
#include_directories(${PROJECT_SOURCE_DIR}/raylib/src)
......
Doxyfile 0 → 100644
View file @ b0598b21
This source diff could not be displayed because it is too large. You can view the blob instead.
Makefile
View file @ b0598b21
......@@ -23,3 +23,7 @@ windows:
clean:
@make -C src clean
docs:
@doxygen Doxyfile
README.md
View file @ b0598b21
......@@ -9,8 +9,8 @@ La particularité du projet est de modéliser le monde comme un réseau de posit
Projet conçu pour une compilation avec GCC et la librairie Raylib. De par la nature de la procédure d'installation et de linkage, Linux est le système d'exploitation à préférer.
⚠️ Bien prendre le temps de lire la section d'installation qui vous concerne ⚠️
⚠️ Bien prendre le temps de lire la section d'installation qui vous concerne ⚠️
### Linux (Debian based, *e.g.* Ubuntu) - Fortement recommandé
---
......@@ -156,7 +156,6 @@ Et voilà !
## Organisation du répertoire
Répertoire:
......
bin/build.sh 0 → 100755
View file @ b0598b21
#!/bin/bash
if [ ! -d "build" ];then
mkdir build
fi
cd build
cmake ..
make
# Outputs
cp nw-viewer ..
bin/install-ubuntu deleted 100755 → 0
View file @ 8593c4e8
#!/bin/bash
# Dependencies:
sudo apt update
sudo apt install -y build-essential git cmake
sudo apt install -y libasound2-dev mesa-common-dev \
libx11-dev libxrandr-dev libxi-dev xorg-dev \
libgl1-mesa-dev libglu1-mesa-dev
# Raylib:
git clone https://github.com/raysan5/raylib.git raylib
cd raylib
git checkout '3.0.0'
mkdir build && cd build
cmake -DSHARED=ON -DSTATIC=ON ..
make
sudo make install
cd ..
# NetWorld:
bin/build
\ No newline at end of file
doc/main-page.md 0 → 100644
View file @ b0598b21
# NetWorld - Documentation
\ No newline at end of file
dpd/include/physac.h 0 → 100644
View file @ b0598b21
/**********************************************************************************************
*
* Physac v1.1 - 2D Physics library for videogames
*
* DESCRIPTION:
*
* Physac is a small 2D physics engine written in pure C. The engine uses a fixed time-step thread loop
* to simluate physics. A physics step contains the following phases: get collision information,
* apply dynamics, collision solving and position correction. It uses a very simple struct for physic
* bodies with a position vector to be used in any 3D rendering API.
*
* CONFIGURATION:
*
* #define PHYSAC_IMPLEMENTATION
* Generates the implementation of the library into the included file.
* If not defined, the library is in header only mode and can be included in other headers
* or source files without problems. But only ONE file should hold the implementation.
*
* #define PHYSAC_STATIC (defined by default)
* The generated implementation will stay private inside implementation file and all
* internal symbols and functions will only be visible inside that file.
*
* #define PHYSAC_DEBUG
* Show debug traces log messages about physic bodies creation/destruction, physic system errors,
* some calculations results and NULL reference exceptions
*
* #define PHYSAC_DEFINE_VECTOR2_TYPE
* Forces library to define struct Vector2 data type (float x; float y)
*
* #define PHYSAC_AVOID_TIMMING_SYSTEM
* Disables internal timming system, used by UpdatePhysics() to launch timmed physic steps,
* it allows just running UpdatePhysics() automatically on a separate thread at a desired time step.
* In case physics steps update needs to be controlled by user with a custom timming mechanism,
* just define this flag and the internal timming mechanism will be avoided, in that case,
* timming libraries are neither required by the module.
*
* #define PHYSAC_MALLOC()
* #define PHYSAC_CALLOC()
* #define PHYSAC_FREE()
* You can define your own malloc/free implementation replacing stdlib.h malloc()/free() functions.
* Otherwise it will include stdlib.h and use the C standard library malloc()/free() function.
*
* COMPILATION:
*
* Use the following code to compile with GCC:
* gcc -o $(NAME_PART).exe $(FILE_NAME) -s -static -lraylib -lopengl32 -lgdi32 -lwinmm -std=c99
*
* VERSIONS HISTORY:
* 1.1 (20-Jan-2021) @raysan5: Library general revision
* Removed threading system (up to the user)
* Support MSVC C++ compilation using CLITERAL()
* Review DEBUG mechanism for TRACELOG() and all TRACELOG() messages
* Review internal variables/functions naming for consistency
* Allow option to avoid internal timming system, to allow app manage the steps
* 1.0 (12-Jun-2017) First release of the library
*
*
* LICENSE: zlib/libpng
*
* Copyright (c) 2016-2021 Victor Fisac (@victorfisac) and Ramon Santamaria (@raysan5)
*
* This software is provided "as-is", without any express or implied warranty. In no event
* will the authors be held liable for any damages arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose, including commercial
* applications, and to alter it and redistribute it freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not claim that you
* wrote the original software. If you use this software in a product, an acknowledgment
* in the product documentation would be appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be misrepresented
* as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*
**********************************************************************************************/
#if !defined(PHYSAC_H)
#define PHYSAC_H
#if defined(PHYSAC_STATIC)
#define PHYSACDEF static // Functions just visible to module including this file
#else
#if defined(__cplusplus)
#define PHYSACDEF extern "C" // Functions visible from other files (no name mangling of functions in C++)
#else
#define PHYSACDEF extern // Functions visible from other files
#endif
#endif
// Allow custom memory allocators
#ifndef PHYSAC_MALLOC
#define PHYSAC_MALLOC(size) malloc(size)
#endif
#ifndef PHYSAC_CALLOC
#define PHYSAC_CALLOC(size, n) calloc(size, n)
#endif
#ifndef PHYSAC_FREE
#define PHYSAC_FREE(ptr) free(ptr)
#endif
//----------------------------------------------------------------------------------
// Defines and Macros
//----------------------------------------------------------------------------------
#define PHYSAC_MAX_BODIES 64 // Maximum number of physic bodies supported
#define PHYSAC_MAX_MANIFOLDS 4096 // Maximum number of physic bodies interactions (64x64)
#define PHYSAC_MAX_VERTICES 24 // Maximum number of vertex for polygons shapes
#define PHYSAC_DEFAULT_CIRCLE_VERTICES 24 // Default number of vertices for circle shapes
#define PHYSAC_COLLISION_ITERATIONS 100
#define PHYSAC_PENETRATION_ALLOWANCE 0.05f
#define PHYSAC_PENETRATION_CORRECTION 0.4f
#define PHYSAC_PI 3.14159265358979323846f
#define PHYSAC_DEG2RAD (PHYSAC_PI/180.0f)
//----------------------------------------------------------------------------------
// Data Types Structure Definition
//----------------------------------------------------------------------------------
#if defined(__STDC__) && __STDC_VERSION__ >= 199901L
#include <stdbool.h>
#endif
typedef enum PhysicsShapeType { PHYSICS_CIRCLE = 0, PHYSICS_POLYGON } PhysicsShapeType;
// Previously defined to be used in PhysicsShape struct as circular dependencies
typedef struct PhysicsBodyData *PhysicsBody;
#if defined(PHYSAC_DEFINE_VECTOR2_TYPE)
// Vector2 type
typedef struct Vector2 {
float x;
float y;
} Vector2;
#endif
// Matrix2x2 type (used for polygon shape rotation matrix)
typedef struct Matrix2x2 {
float m00;
float m01;
float m10;
float m11;
} Matrix2x2;
typedef struct PhysicsVertexData {
unsigned int vertexCount; // Vertex count (positions and normals)
Vector2 positions[PHYSAC_MAX_VERTICES]; // Vertex positions vectors
Vector2 normals[PHYSAC_MAX_VERTICES]; // Vertex normals vectors
} PhysicsVertexData;
typedef struct PhysicsShape {
PhysicsShapeType type; // Shape type (circle or polygon)
PhysicsBody body; // Shape physics body data pointer
PhysicsVertexData vertexData; // Shape vertices data (used for polygon shapes)
float radius; // Shape radius (used for circle shapes)
Matrix2x2 transform; // Vertices transform matrix 2x2
} PhysicsShape;
typedef struct PhysicsBodyData {
unsigned int id; // Unique identifier
bool enabled; // Enabled dynamics state (collisions are calculated anyway)
Vector2 position; // Physics body shape pivot
Vector2 velocity; // Current linear velocity applied to position
Vector2 force; // Current linear force (reset to 0 every step)
float angularVelocity; // Current angular velocity applied to orient
float torque; // Current angular force (reset to 0 every step)
float orient; // Rotation in radians
float inertia; // Moment of inertia
float inverseInertia; // Inverse value of inertia
float mass; // Physics body mass
float inverseMass; // Inverse value of mass
float staticFriction; // Friction when the body has not movement (0 to 1)
float dynamicFriction; // Friction when the body has movement (0 to 1)
float restitution; // Restitution coefficient of the body (0 to 1)
bool useGravity; // Apply gravity force to dynamics
bool isGrounded; // Physics grounded on other body state
bool freezeOrient; // Physics rotation constraint
PhysicsShape shape; // Physics body shape information (type, radius, vertices, transform)
} PhysicsBodyData;
typedef struct PhysicsManifoldData {
unsigned int id; // Unique identifier
PhysicsBody bodyA; // Manifold first physics body reference
PhysicsBody bodyB; // Manifold second physics body reference
float penetration; // Depth of penetration from collision
Vector2 normal; // Normal direction vector from 'a' to 'b'
Vector2 contacts[2]; // Points of contact during collision
unsigned int contactsCount; // Current collision number of contacts
float restitution; // Mixed restitution during collision
float dynamicFriction; // Mixed dynamic friction during collision
float staticFriction; // Mixed static friction during collision
} PhysicsManifoldData, *PhysicsManifold;
#if defined(__cplusplus)
extern "C" { // Prevents name mangling of functions
#endif
//----------------------------------------------------------------------------------
// Module Functions Declaration
//----------------------------------------------------------------------------------
// Physics system management
PHYSACDEF void InitPhysics(void); // Initializes physics system
PHYSACDEF void UpdatePhysics(void); // Update physics system
PHYSACDEF void ResetPhysics(void); // Reset physics system (global variables)
PHYSACDEF void ClosePhysics(void); // Close physics system and unload used memory
PHYSACDEF void SetPhysicsTimeStep(double delta); // Sets physics fixed time step in milliseconds. 1.666666 by default
PHYSACDEF void SetPhysicsGravity(float x, float y); // Sets physics global gravity force
// Physic body creation/destroy
PHYSACDEF PhysicsBody CreatePhysicsBodyCircle(Vector2 pos, float radius, float density); // Creates a new circle physics body with generic parameters
PHYSACDEF PhysicsBody CreatePhysicsBodyRectangle(Vector2 pos, float width, float height, float density); // Creates a new rectangle physics body with generic parameters
PHYSACDEF PhysicsBody CreatePhysicsBodyPolygon(Vector2 pos, float radius, int sides, float density); // Creates a new polygon physics body with generic parameters
PHYSACDEF void DestroyPhysicsBody(PhysicsBody body); // Destroy a physics body
// Physic body forces
PHYSACDEF void PhysicsAddForce(PhysicsBody body, Vector2 force); // Adds a force to a physics body
PHYSACDEF void PhysicsAddTorque(PhysicsBody body, float amount); // Adds an angular force to a physics body
PHYSACDEF void PhysicsShatter(PhysicsBody body, Vector2 position, float force); // Shatters a polygon shape physics body to little physics bodies with explosion force
PHYSACDEF void SetPhysicsBodyRotation(PhysicsBody body, float radians); // Sets physics body shape transform based on radians parameter
// Query physics info
PHYSACDEF PhysicsBody GetPhysicsBody(int index); // Returns a physics body of the bodies pool at a specific index
PHYSACDEF int GetPhysicsBodiesCount(void); // Returns the current amount of created physics bodies
PHYSACDEF int GetPhysicsShapeType(int index); // Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON)
PHYSACDEF int GetPhysicsShapeVerticesCount(int index); // Returns the amount of vertices of a physics body shape
PHYSACDEF Vector2 GetPhysicsShapeVertex(PhysicsBody body, int vertex); // Returns transformed position of a body shape (body position + vertex transformed position)
#if defined(__cplusplus)
}
#endif
#endif // PHYSAC_H
/***********************************************************************************
*
* PHYSAC IMPLEMENTATION
*
************************************************************************************/
#if defined(PHYSAC_IMPLEMENTATION)
// Support TRACELOG macros
#if defined(PHYSAC_DEBUG)
#include <stdio.h> // Required for: printf()
#define TRACELOG(...) printf(__VA_ARGS__)
#else
#define TRACELOG(...) (void)0;
#endif
#include <stdlib.h> // Required for: malloc(), calloc(), free()
#include <math.h> // Required for: cosf(), sinf(), fabs(), sqrtf()
#if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
// Time management functionality
#include <time.h> // Required for: time(), clock_gettime()
#if defined(_WIN32)
// Functions required to query time on Windows
int __stdcall QueryPerformanceCounter(unsigned long long int *lpPerformanceCount);
int __stdcall QueryPerformanceFrequency(unsigned long long int *lpFrequency);
#endif
#if defined(__linux__) || defined(__FreeBSD__)
#if _POSIX_C_SOURCE < 199309L
#undef _POSIX_C_SOURCE
#define _POSIX_C_SOURCE 199309L // Required for CLOCK_MONOTONIC if compiled with c99 without gnu ext.
#endif
#include <sys/time.h> // Required for: timespec
#endif
#if defined(__APPLE__) // macOS also defines __MACH__
#include <mach/mach_time.h> // Required for: mach_absolute_time()
#endif
#endif
// NOTE: MSVC C++ compiler does not support compound literals (C99 feature)
// Plain structures in C++ (without constructors) can be initialized from { } initializers.
#if defined(__cplusplus)
#define CLITERAL(type) type
#else
#define CLITERAL(type) (type)
#endif
//----------------------------------------------------------------------------------
// Defines and Macros
//----------------------------------------------------------------------------------
#define PHYSAC_MIN(a,b) (((a)<(b))?(a):(b))
#define PHYSAC_MAX(a,b) (((a)>(b))?(a):(b))
#define PHYSAC_FLT_MAX 3.402823466e+38f
#define PHYSAC_EPSILON 0.000001f
#define PHYSAC_K 1.0f/3.0f
#define PHYSAC_VECTOR_ZERO CLITERAL(Vector2){ 0.0f, 0.0f }
//----------------------------------------------------------------------------------
// Global Variables Definition
//----------------------------------------------------------------------------------
static double deltaTime = 1.0/60.0/10.0 * 1000; // Delta time in milliseconds used for physics steps
#if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
// Time measure variables
static double baseClockTicks = 0.0; // Offset clock ticks for MONOTONIC clock
static unsigned long long int frequency = 0; // Hi-res clock frequency
static double startTime = 0.0; // Start time in milliseconds
static double currentTime = 0.0; // Current time in milliseconds
#endif
// Physics system configuration
static PhysicsBody bodies[PHYSAC_MAX_BODIES]; // Physics bodies pointers array
static unsigned int physicsBodiesCount = 0; // Physics world current bodies counter
static PhysicsManifold contacts[PHYSAC_MAX_MANIFOLDS]; // Physics bodies pointers array
static unsigned int physicsManifoldsCount = 0; // Physics world current manifolds counter
static Vector2 gravityForce = { 0.0f, 9.81f }; // Physics world gravity force
// Utilities variables
static unsigned int usedMemory = 0; // Total allocated dynamic memory
//----------------------------------------------------------------------------------
// Module Internal Functions Declaration
//----------------------------------------------------------------------------------
#if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
// Timming measure functions
static void InitTimer(void); // Initializes hi-resolution MONOTONIC timer
static unsigned long long int GetClockTicks(void); // Get hi-res MONOTONIC time measure in mseconds
static double GetCurrentTime(void); // Get current time measure in milliseconds
#endif
static void UpdatePhysicsStep(void); // Update physics step (dynamics, collisions and position corrections)
static int FindAvailableBodyIndex(); // Finds a valid index for a new physics body initialization
static int FindAvailableManifoldIndex(); // Finds a valid index for a new manifold initialization
static PhysicsVertexData CreateDefaultPolygon(float radius, int sides); // Creates a random polygon shape with max vertex distance from polygon pivot
static PhysicsVertexData CreateRectanglePolygon(Vector2 pos, Vector2 size); // Creates a rectangle polygon shape based on a min and max positions
static void InitializePhysicsManifolds(PhysicsManifold manifold); // Initializes physics manifolds to solve collisions
static PhysicsManifold CreatePhysicsManifold(PhysicsBody a, PhysicsBody b); // Creates a new physics manifold to solve collision
static void DestroyPhysicsManifold(PhysicsManifold manifold); // Unitializes and destroys a physics manifold
static void SolvePhysicsManifold(PhysicsManifold manifold); // Solves a created physics manifold between two physics bodies
static void SolveCircleToCircle(PhysicsManifold manifold); // Solves collision between two circle shape physics bodies
static void SolveCircleToPolygon(PhysicsManifold manifold); // Solves collision between a circle to a polygon shape physics bodies
static void SolvePolygonToCircle(PhysicsManifold manifold); // Solves collision between a polygon to a circle shape physics bodies
static void SolvePolygonToPolygon(PhysicsManifold manifold); // Solves collision between two polygons shape physics bodies
static void IntegratePhysicsForces(PhysicsBody body); // Integrates physics forces into velocity
static void IntegratePhysicsVelocity(PhysicsBody body); // Integrates physics velocity into position and forces
static void IntegratePhysicsImpulses(PhysicsManifold manifold); // Integrates physics collisions impulses to solve collisions
static void CorrectPhysicsPositions(PhysicsManifold manifold); // Corrects physics bodies positions based on manifolds collision information
static void FindIncidentFace(Vector2 *v0, Vector2 *v1, PhysicsShape ref, PhysicsShape inc, int index); // Finds two polygon shapes incident face
static float FindAxisLeastPenetration(int *faceIndex, PhysicsShape shapeA, PhysicsShape shapeB); // Finds polygon shapes axis least penetration
// Math required functions
static Vector2 MathVector2Product(Vector2 vector, float value); // Returns the product of a vector and a value
static float MathVector2CrossProduct(Vector2 v1, Vector2 v2); // Returns the cross product of two vectors
static float MathVector2SqrLen(Vector2 vector); // Returns the len square root of a vector
static float MathVector2DotProduct(Vector2 v1, Vector2 v2); // Returns the dot product of two vectors
static inline float MathVector2SqrDistance(Vector2 v1, Vector2 v2); // Returns the square root of distance between two vectors
static void MathVector2Normalize(Vector2 *vector); // Returns the normalized values of a vector
static Vector2 MathVector2Add(Vector2 v1, Vector2 v2); // Returns the sum of two given vectors
static Vector2 MathVector2Subtract(Vector2 v1, Vector2 v2); // Returns the subtract of two given vectors
static Matrix2x2 MathMatFromRadians(float radians); // Returns a matrix 2x2 from a given radians value
static inline Matrix2x2 MathMatTranspose(Matrix2x2 matrix); // Returns the transpose of a given matrix 2x2
static inline Vector2 MathMatVector2Product(Matrix2x2 matrix, Vector2 vector); // Returns product between matrix 2x2 and vector
static int MathVector2Clip(Vector2 normal, Vector2 *faceA, Vector2 *faceB, float clip); // Returns clipping value based on a normal and two faces
static Vector2 MathTriangleBarycenter(Vector2 v1, Vector2 v2, Vector2 v3); // Returns the barycenter of a triangle given by 3 points
//----------------------------------------------------------------------------------
// Module Functions Definition
//----------------------------------------------------------------------------------
// Initializes physics values, pointers and creates physics loop thread
PHYSACDEF void InitPhysics(void)
{
#if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
// Initialize high resolution timer
InitTimer();
#endif
TRACELOG("[PHYSAC] Physics module initialized successfully\n");
}
// Sets physics global gravity force
PHYSACDEF void SetPhysicsGravity(float x, float y)
{
gravityForce.x = x;
gravityForce.y = y;
}
// Creates a new circle physics body with generic parameters
PHYSACDEF PhysicsBody CreatePhysicsBodyCircle(Vector2 pos, float radius, float density)
{
PhysicsBody body = CreatePhysicsBodyPolygon(pos, radius, PHYSAC_DEFAULT_CIRCLE_VERTICES, density);
return body;
}
// Creates a new rectangle physics body with generic parameters
PHYSACDEF PhysicsBody CreatePhysicsBodyRectangle(Vector2 pos, float width, float height, float density)
{
// NOTE: Make sure body data is initialized to 0
PhysicsBody body = (PhysicsBody)PHYSAC_CALLOC(sizeof(PhysicsBodyData), 1);
usedMemory += sizeof(PhysicsBodyData);
int id = FindAvailableBodyIndex();
if (id != -1)
{
// Initialize new body with generic values
body->id = id;
body->enabled = true;
body->position = pos;
body->shape.type = PHYSICS_POLYGON;
body->shape.body = body;
body->shape.transform = MathMatFromRadians(0.0f);
body->shape.vertexData = CreateRectanglePolygon(pos, CLITERAL(Vector2){ width, height });
// Calculate centroid and moment of inertia
Vector2 center = { 0.0f, 0.0f };
float area = 0.0f;
float inertia = 0.0f;
for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++)
{
// Triangle vertices, third vertex implied as (0, 0)
Vector2 p1 = body->shape.vertexData.positions[i];
unsigned int nextIndex = (((i + 1) < body->shape.vertexData.vertexCount) ? (i + 1) : 0);
Vector2 p2 = body->shape.vertexData.positions[nextIndex];
float D = MathVector2CrossProduct(p1, p2);
float triangleArea = D/2;
area += triangleArea;
// Use area to weight the centroid average, not just vertex position
center.x += triangleArea*PHYSAC_K*(p1.x + p2.x);
center.y += triangleArea*PHYSAC_K*(p1.y + p2.y);
float intx2 = p1.x*p1.x + p2.x*p1.x + p2.x*p2.x;
float inty2 = p1.y*p1.y + p2.y*p1.y + p2.y*p2.y;
inertia += (0.25f*PHYSAC_K*D)*(intx2 + inty2);
}
center.x *= 1.0f/area;
center.y *= 1.0f/area;
// Translate vertices to centroid (make the centroid (0, 0) for the polygon in model space)
// Note: this is not really necessary
for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++)
{
body->shape.vertexData.positions[i].x -= center.x;
body->shape.vertexData.positions[i].y -= center.y;
}
body->mass = density*area;
body->inverseMass = ((body->mass != 0.0f) ? 1.0f/body->mass : 0.0f);
body->inertia = density*inertia;
body->inverseInertia = ((body->inertia != 0.0f) ? 1.0f/body->inertia : 0.0f);
body->staticFriction = 0.4f;
body->dynamicFriction = 0.2f;
body->restitution = 0.0f;
body->useGravity = true;
body->isGrounded = false;
body->freezeOrient = false;
// Add new body to bodies pointers array and update bodies count
bodies[physicsBodiesCount] = body;
physicsBodiesCount++;
TRACELOG("[PHYSAC] Physic body created successfully (id: %i)\n", body->id);
}
else TRACELOG("[PHYSAC] Physic body could not be created, PHYSAC_MAX_BODIES reached\n");
return body;
}
// Creates a new polygon physics body with generic parameters
PHYSACDEF PhysicsBody CreatePhysicsBodyPolygon(Vector2 pos, float radius, int sides, float density)
{
PhysicsBody body = (PhysicsBody)PHYSAC_MALLOC(sizeof(PhysicsBodyData));
usedMemory += sizeof(PhysicsBodyData);
int id = FindAvailableBodyIndex();
if (id != -1)
{
// Initialize new body with generic values
body->id = id;
body->enabled = true;
body->position = pos;
body->velocity = PHYSAC_VECTOR_ZERO;
body->force = PHYSAC_VECTOR_ZERO;
body->angularVelocity = 0.0f;
body->torque = 0.0f;
body->orient = 0.0f;
body->shape.type = PHYSICS_POLYGON;
body->shape.body = body;
body->shape.transform = MathMatFromRadians(0.0f);
body->shape.vertexData = CreateDefaultPolygon(radius, sides);
// Calculate centroid and moment of inertia
Vector2 center = { 0.0f, 0.0f };
float area = 0.0f;
float inertia = 0.0f;
for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++)
{
// Triangle vertices, third vertex implied as (0, 0)
Vector2 position1 = body->shape.vertexData.positions[i];
unsigned int nextIndex = (((i + 1) < body->shape.vertexData.vertexCount) ? (i + 1) : 0);
Vector2 position2 = body->shape.vertexData.positions[nextIndex];
float cross = MathVector2CrossProduct(position1, position2);
float triangleArea = cross/2;
area += triangleArea;
// Use area to weight the centroid average, not just vertex position
center.x += triangleArea*PHYSAC_K*(position1.x + position2.x);
center.y += triangleArea*PHYSAC_K*(position1.y + position2.y);
float intx2 = position1.x*position1.x + position2.x*position1.x + position2.x*position2.x;
float inty2 = position1.y*position1.y + position2.y*position1.y + position2.y*position2.y;
inertia += (0.25f*PHYSAC_K*cross)*(intx2 + inty2);
}
center.x *= 1.0f/area;
center.y *= 1.0f/area;
// Translate vertices to centroid (make the centroid (0, 0) for the polygon in model space)
// Note: this is not really necessary
for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++)
{
body->shape.vertexData.positions[i].x -= center.x;
body->shape.vertexData.positions[i].y -= center.y;
}
body->mass = density*area;
body->inverseMass = ((body->mass != 0.0f) ? 1.0f/body->mass : 0.0f);
body->inertia = density*inertia;
body->inverseInertia = ((body->inertia != 0.0f) ? 1.0f/body->inertia : 0.0f);
body->staticFriction = 0.4f;
body->dynamicFriction = 0.2f;
body->restitution = 0.0f;
body->useGravity = true;
body->isGrounded = false;
body->freezeOrient = false;
// Add new body to bodies pointers array and update bodies count
bodies[physicsBodiesCount] = body;
physicsBodiesCount++;
TRACELOG("[PHYSAC] Physic body created successfully (id: %i)\n", body->id);
}
else TRACELOG("[PHYSAC] Physics body could not be created, PHYSAC_MAX_BODIES reached\n");
return body;
}
// Adds a force to a physics body
PHYSACDEF void PhysicsAddForce(PhysicsBody body, Vector2 force)
{
if (body != NULL) body->force = MathVector2Add(body->force, force);
}
// Adds an angular force to a physics body
PHYSACDEF void PhysicsAddTorque(PhysicsBody body, float amount)
{
if (body != NULL) body->torque += amount;
}
// Shatters a polygon shape physics body to little physics bodies with explosion force
PHYSACDEF void PhysicsShatter(PhysicsBody body, Vector2 position, float force)
{
if (body != NULL)
{
if (body->shape.type == PHYSICS_POLYGON)
{
PhysicsVertexData vertexData = body->shape.vertexData;
bool collision = false;
for (unsigned int i = 0; i < vertexData.vertexCount; i++)
{
Vector2 positionA = body->position;
Vector2 positionB = MathMatVector2Product(body->shape.transform, MathVector2Add(body->position, vertexData.positions[i]));
unsigned int nextIndex = (((i + 1) < vertexData.vertexCount) ? (i + 1) : 0);
Vector2 positionC = MathMatVector2Product(body->shape.transform, MathVector2Add(body->position, vertexData.positions[nextIndex]));
// Check collision between each triangle
float alpha = ((positionB.y - positionC.y)*(position.x - positionC.x) + (positionC.x - positionB.x)*(position.y - positionC.y))/
((positionB.y - positionC.y)*(positionA.x - positionC.x) + (positionC.x - positionB.x)*(positionA.y - positionC.y));
float beta = ((positionC.y - positionA.y)*(position.x - positionC.x) + (positionA.x - positionC.x)*(position.y - positionC.y))/
((positionB.y - positionC.y)*(positionA.x - positionC.x) + (positionC.x - positionB.x)*(positionA.y - positionC.y));
float gamma = 1.0f - alpha - beta;
if ((alpha > 0.0f) && (beta > 0.0f) & (gamma > 0.0f))
{
collision = true;
break;
}
}
if (collision)
{
int count = vertexData.vertexCount;
Vector2 bodyPos = body->position;
Vector2 *vertices = (Vector2 *)PHYSAC_MALLOC(sizeof(Vector2)*count);
Matrix2x2 trans = body->shape.transform;
for (int i = 0; i < count; i++) vertices[i] = vertexData.positions[i];
// Destroy shattered physics body
DestroyPhysicsBody(body);
for (int i = 0; i < count; i++)
{
int nextIndex = (((i + 1) < count) ? (i + 1) : 0);
Vector2 center = MathTriangleBarycenter(vertices[i], vertices[nextIndex], PHYSAC_VECTOR_ZERO);
center = MathVector2Add(bodyPos, center);
Vector2 offset = MathVector2Subtract(center, bodyPos);
PhysicsBody body = CreatePhysicsBodyPolygon(center, 10, 3, 10); // Create polygon physics body with relevant values
PhysicsVertexData vertexData = { 0 };
vertexData.vertexCount = 3;
vertexData.positions[0] = MathVector2Subtract(vertices[i], offset);
vertexData.positions[1] = MathVector2Subtract(vertices[nextIndex], offset);
vertexData.positions[2] = MathVector2Subtract(position, center);
// Separate vertices to avoid unnecessary physics collisions
vertexData.positions[0].x *= 0.95f;
vertexData.positions[0].y *= 0.95f;
vertexData.positions[1].x *= 0.95f;
vertexData.positions[1].y *= 0.95f;
vertexData.positions[2].x *= 0.95f;
vertexData.positions[2].y *= 0.95f;
// Calculate polygon faces normals
for (unsigned int j = 0; j < vertexData.vertexCount; j++)
{
unsigned int nextVertex = (((j + 1) < vertexData.vertexCount) ? (j + 1) : 0);
Vector2 face = MathVector2Subtract(vertexData.positions[nextVertex], vertexData.positions[j]);
vertexData.normals[j] = CLITERAL(Vector2){ face.y, -face.x };
MathVector2Normalize(&vertexData.normals[j]);
}
// Apply computed vertex data to new physics body shape
body->shape.vertexData = vertexData;
body->shape.transform = trans;
// Calculate centroid and moment of inertia
center = PHYSAC_VECTOR_ZERO;
float area = 0.0f;
float inertia = 0.0f;
for (unsigned int j = 0; j < body->shape.vertexData.vertexCount; j++)
{
// Triangle vertices, third vertex implied as (0, 0)
Vector2 p1 = body->shape.vertexData.positions[j];
unsigned int nextVertex = (((j + 1) < body->shape.vertexData.vertexCount) ? (j + 1) : 0);
Vector2 p2 = body->shape.vertexData.positions[nextVertex];
float D = MathVector2CrossProduct(p1, p2);
float triangleArea = D/2;
area += triangleArea;
// Use area to weight the centroid average, not just vertex position
center.x += triangleArea*PHYSAC_K*(p1.x + p2.x);
center.y += triangleArea*PHYSAC_K*(p1.y + p2.y);
float intx2 = p1.x*p1.x + p2.x*p1.x + p2.x*p2.x;
float inty2 = p1.y*p1.y + p2.y*p1.y + p2.y*p2.y;
inertia += (0.25f*PHYSAC_K*D)*(intx2 + inty2);
}
center.x *= 1.0f/area;
center.y *= 1.0f/area;
body->mass = area;
body->inverseMass = ((body->mass != 0.0f) ? 1.0f/body->mass : 0.0f);
body->inertia = inertia;
body->inverseInertia = ((body->inertia != 0.0f) ? 1.0f/body->inertia : 0.0f);
// Calculate explosion force direction
Vector2 pointA = body->position;
Vector2 pointB = MathVector2Subtract(vertexData.positions[1], vertexData.positions[0]);
pointB.x /= 2.0f;
pointB.y /= 2.0f;
Vector2 forceDirection = MathVector2Subtract(MathVector2Add(pointA, MathVector2Add(vertexData.positions[0], pointB)), body->position);
MathVector2Normalize(&forceDirection);
forceDirection.x *= force;
forceDirection.y *= force;
// Apply force to new physics body
PhysicsAddForce(body, forceDirection);
}
PHYSAC_FREE(vertices);
}
}
}
else TRACELOG("[PHYSAC] WARNING: PhysicsShatter: NULL physic body\n");
}
// Returns the current amount of created physics bodies
PHYSACDEF int GetPhysicsBodiesCount(void)
{
return physicsBodiesCount;
}
// Returns a physics body of the bodies pool at a specific index
PHYSACDEF PhysicsBody GetPhysicsBody(int index)
{
PhysicsBody body = NULL;
if (index < (int)physicsBodiesCount)
{
body = bodies[index];
if (body == NULL) TRACELOG("[PHYSAC] WARNING: GetPhysicsBody: NULL physic body\n");
}
else TRACELOG("[PHYSAC] WARNING: Physic body index is out of bounds\n");
return body;
}
// Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON)
PHYSACDEF int GetPhysicsShapeType(int index)
{
int result = -1;
if (index < (int)physicsBodiesCount)
{
PhysicsBody body = bodies[index];
if (body != NULL) result = body->shape.type;
else TRACELOG("[PHYSAC] WARNING: GetPhysicsShapeType: NULL physic body\n");
}
else TRACELOG("[PHYSAC] WARNING: Physic body index is out of bounds\n");
return result;
}
// Returns the amount of vertices of a physics body shape
PHYSACDEF int GetPhysicsShapeVerticesCount(int index)
{
int result = 0;
if (index < (int)physicsBodiesCount)
{
PhysicsBody body = bodies[index];
if (body != NULL)
{
switch (body->shape.type)
{
case PHYSICS_CIRCLE: result = PHYSAC_DEFAULT_CIRCLE_VERTICES; break;
case PHYSICS_POLYGON: result = body->shape.vertexData.vertexCount; break;
default: break;
}
}
else TRACELOG("[PHYSAC] WARNING: GetPhysicsShapeVerticesCount: NULL physic body\n");
}
else TRACELOG("[PHYSAC] WARNING: Physic body index is out of bounds\n");
return result;
}
// Returns transformed position of a body shape (body position + vertex transformed position)
PHYSACDEF Vector2 GetPhysicsShapeVertex(PhysicsBody body, int vertex)
{
Vector2 position = { 0.0f, 0.0f };
if (body != NULL)
{
switch (body->shape.type)
{
case PHYSICS_CIRCLE:
{
position.x = body->position.x + cosf(360.0f/PHYSAC_DEFAULT_CIRCLE_VERTICES*vertex*PHYSAC_DEG2RAD)*body->shape.radius;
position.y = body->position.y + sinf(360.0f/PHYSAC_DEFAULT_CIRCLE_VERTICES*vertex*PHYSAC_DEG2RAD)*body->shape.radius;
} break;
case PHYSICS_POLYGON:
{
PhysicsVertexData vertexData = body->shape.vertexData;
position = MathVector2Add(body->position, MathMatVector2Product(body->shape.transform, vertexData.positions[vertex]));
} break;
default: break;
}
}
else TRACELOG("[PHYSAC] WARNING: GetPhysicsShapeVertex: NULL physic body\n");
return position;
}
// Sets physics body shape transform based on radians parameter
PHYSACDEF void SetPhysicsBodyRotation(PhysicsBody body, float radians)
{
if (body != NULL)
{
body->orient = radians;
if (body->shape.type == PHYSICS_POLYGON) body->shape.transform = MathMatFromRadians(radians);
}
}
// Unitializes and destroys a physics body
PHYSACDEF void DestroyPhysicsBody(PhysicsBody body)
{
if (body != NULL)
{
int id = body->id;
int index = -1;
for (unsigned int i = 0; i < physicsBodiesCount; i++)
{
if (bodies[i]->id == id)
{
index = i;
break;
}
}
if (index == -1)
{
TRACELOG("[PHYSAC] WARNING: Requested body (id: %i) can not be found\n", id);
return; // Prevent access to index -1
}
// Free body allocated memory
PHYSAC_FREE(body);
usedMemory -= sizeof(PhysicsBodyData);
bodies[index] = NULL;
// Reorder physics bodies pointers array and its catched index
for (unsigned int i = index; i < physicsBodiesCount; i++)
{
if ((i + 1) < physicsBodiesCount) bodies[i] = bodies[i + 1];
}
// Update physics bodies count
physicsBodiesCount--;
TRACELOG("[PHYSAC] Physic body destroyed successfully (id: %i)\n", id);
}
else TRACELOG("[PHYSAC] WARNING: DestroyPhysicsBody: NULL physic body\n");
}
// Destroys created physics bodies and manifolds and resets global values
PHYSACDEF void ResetPhysics(void)
{
if (physicsBodiesCount > 0)
{
// Unitialize physics bodies dynamic memory allocations
for (unsigned int i = physicsBodiesCount - 1; i >= 0; i--)
{
PhysicsBody body = bodies[i];
if (body != NULL)
{
PHYSAC_FREE(body);
bodies[i] = NULL;
usedMemory -= sizeof(PhysicsBodyData);
}
}
physicsBodiesCount = 0;
}
if (physicsManifoldsCount > 0)
{
// Unitialize physics manifolds dynamic memory allocations
for (unsigned int i = physicsManifoldsCount - 1; i >= 0; i--)
{
PhysicsManifold manifold = contacts[i];
if (manifold != NULL)
{
PHYSAC_FREE(manifold);
contacts[i] = NULL;
usedMemory -= sizeof(PhysicsManifoldData);
}
}
physicsManifoldsCount = 0;
}
TRACELOG("[PHYSAC] Physics module reseted successfully\n");
}
// Unitializes physics pointers and exits physics loop thread
PHYSACDEF void ClosePhysics(void)
{
// Unitialize physics manifolds dynamic memory allocations
if (physicsManifoldsCount > 0)
{
for (unsigned int i = physicsManifoldsCount - 1; i >= 0; i--)
DestroyPhysicsManifold(contacts[i]);
}
// Unitialize physics bodies dynamic memory allocations
if (physicsBodiesCount > 0)
{
for (unsigned int i = physicsBodiesCount - 1; i >= 0; i--)
DestroyPhysicsBody(bodies[i]);
}
// Trace log info
if ((physicsBodiesCount > 0) || (usedMemory != 0))
{
TRACELOG("[PHYSAC] WARNING: Physics module closed with unallocated bodies (BODIES: %i, MEMORY: %i bytes)\n", physicsBodiesCount, usedMemory);
}
else if ((physicsManifoldsCount > 0) || (usedMemory != 0))
{
TRACELOG("[PHYSAC] WARNING: Pysics module closed with unallocated manifolds (MANIFOLDS: %i, MEMORY: %i bytes)\n", physicsManifoldsCount, usedMemory);
}
else TRACELOG("[PHYSAC] Physics module closed successfully\n");
}
//----------------------------------------------------------------------------------
// Module Internal Functions Definition
//----------------------------------------------------------------------------------
// Finds a valid index for a new physics body initialization
static int FindAvailableBodyIndex()
{
int index = -1;
for (int i = 0; i < PHYSAC_MAX_BODIES; i++)
{
int currentId = i;
// Check if current id already exist in other physics body
for (unsigned int k = 0; k < physicsBodiesCount; k++)
{
if (bodies[k]->id == currentId)
{
currentId++;
break;
}
}
// If it is not used, use it as new physics body id
if (currentId == (int)i)
{
index = (int)i;
break;
}
}
return index;
}
// Creates a default polygon shape with max vertex distance from polygon pivot
static PhysicsVertexData CreateDefaultPolygon(float radius, int sides)
{
PhysicsVertexData data = { 0 };
data.vertexCount = sides;
// Calculate polygon vertices positions
for (unsigned int i = 0; i < data.vertexCount; i++)
{
data.positions[i].x = (float)cosf(360.0f/sides*i*PHYSAC_DEG2RAD)*radius;
data.positions[i].y = (float)sinf(360.0f/sides*i*PHYSAC_DEG2RAD)*radius;
}
// Calculate polygon faces normals
for (int i = 0; i < (int)data.vertexCount; i++)
{
int nextIndex = (((i + 1) < sides) ? (i + 1) : 0);
Vector2 face = MathVector2Subtract(data.positions[nextIndex], data.positions[i]);
data.normals[i] = CLITERAL(Vector2){ face.y, -face.x };
MathVector2Normalize(&data.normals[i]);
}
return data;
}
// Creates a rectangle polygon shape based on a min and max positions
static PhysicsVertexData CreateRectanglePolygon(Vector2 pos, Vector2 size)
{
PhysicsVertexData data = { 0 };
data.vertexCount = 4;
// Calculate polygon vertices positions
data.positions[0] = CLITERAL(Vector2){ pos.x + size.x/2, pos.y - size.y/2 };
data.positions[1] = CLITERAL(Vector2){ pos.x + size.x/2, pos.y + size.y/2 };
data.positions[2] = CLITERAL(Vector2){ pos.x - size.x/2, pos.y + size.y/2 };
data.positions[3] = CLITERAL(Vector2){ pos.x - size.x/2, pos.y - size.y/2 };
// Calculate polygon faces normals
for (unsigned int i = 0; i < data.vertexCount; i++)
{
int nextIndex = (((i + 1) < data.vertexCount) ? (i + 1) : 0);
Vector2 face = MathVector2Subtract(data.positions[nextIndex], data.positions[i]);
data.normals[i] = CLITERAL(Vector2){ face.y, -face.x };
MathVector2Normalize(&data.normals[i]);
}
return data;
}
// Update physics step (dynamics, collisions and position corrections)
void UpdatePhysicsStep(void)
{
// Clear previous generated collisions information
for (int i = (int)physicsManifoldsCount - 1; i >= 0; i--)
{
PhysicsManifold manifold = contacts[i];
if (manifold != NULL) DestroyPhysicsManifold(manifold);
}
// Reset physics bodies grounded state
for (unsigned int i = 0; i < physicsBodiesCount; i++)
{
PhysicsBody body = bodies[i];
body->isGrounded = false;
}
// Generate new collision information
for (unsigned int i = 0; i < physicsBodiesCount; i++)
{
PhysicsBody bodyA = bodies[i];
if (bodyA != NULL)
{
for (unsigned int j = i + 1; j < physicsBodiesCount; j++)
{
PhysicsBody bodyB = bodies[j];
if (bodyB != NULL)
{
if ((bodyA->inverseMass == 0) && (bodyB->inverseMass == 0)) continue;
PhysicsManifold manifold = CreatePhysicsManifold(bodyA, bodyB);
SolvePhysicsManifold(manifold);
if (manifold->contactsCount > 0)
{
// Create a new manifold with same information as previously solved manifold and add it to the manifolds pool last slot
PhysicsManifold manifold = CreatePhysicsManifold(bodyA, bodyB);
manifold->penetration = manifold->penetration;
manifold->normal = manifold->normal;
manifold->contacts[0] = manifold->contacts[0];
manifold->contacts[1] = manifold->contacts[1];
manifold->contactsCount = manifold->contactsCount;
manifold->restitution = manifold->restitution;
manifold->dynamicFriction = manifold->dynamicFriction;
manifold->staticFriction = manifold->staticFriction;
}
}
}
}
}
// Integrate forces to physics bodies
for (unsigned int i = 0; i < physicsBodiesCount; i++)
{
PhysicsBody body = bodies[i];
if (body != NULL) IntegratePhysicsForces(body);
}
// Initialize physics manifolds to solve collisions
for (unsigned int i = 0; i < physicsManifoldsCount; i++)
{
PhysicsManifold manifold = contacts[i];
if (manifold != NULL) InitializePhysicsManifolds(manifold);
}
// Integrate physics collisions impulses to solve collisions
for (unsigned int i = 0; i < PHYSAC_COLLISION_ITERATIONS; i++)
{
for (unsigned int j = 0; j < physicsManifoldsCount; j++)
{
PhysicsManifold manifold = contacts[i];
if (manifold != NULL) IntegratePhysicsImpulses(manifold);
}
}
// Integrate velocity to physics bodies
for (unsigned int i = 0; i < physicsBodiesCount; i++)
{
PhysicsBody body = bodies[i];
if (body != NULL) IntegratePhysicsVelocity(body);
}
// Correct physics bodies positions based on manifolds collision information
for (unsigned int i = 0; i < physicsManifoldsCount; i++)
{
PhysicsManifold manifold = contacts[i];
if (manifold != NULL) CorrectPhysicsPositions(manifold);
}
// Clear physics bodies forces
for (unsigned int i = 0; i < physicsBodiesCount; i++)
{
PhysicsBody body = bodies[i];
if (body != NULL)
{
body->force = PHYSAC_VECTOR_ZERO;
body->torque = 0.0f;
}
}
}
// Update physics system
// Physics steps are launched at a fixed time step if enabled
PHYSACDEF void UpdatePhysics(void)
{
#if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
static double deltaTimeAccumulator = 0.0;
// Calculate current time (ms)
currentTime = GetCurrentTime();
// Calculate current delta time (ms)
const double delta = currentTime - startTime;
// Store the time elapsed since the last frame began
deltaTimeAccumulator += delta;
// Fixed time stepping loop
while (deltaTimeAccumulator >= deltaTime)
{
UpdatePhysicsStep();
deltaTimeAccumulator -= deltaTime;
}
// Record the starting of this frame
startTime = currentTime;
#else
UpdatePhysicsStep();
#endif
}
PHYSACDEF void SetPhysicsTimeStep(double delta)
{
deltaTime = delta;
}
// Finds a valid index for a new manifold initialization
static int FindAvailableManifoldIndex()
{
int index = -1;
for (int i = 0; i < PHYSAC_MAX_MANIFOLDS; i++)
{
int currentId = i;
// Check if current id already exist in other physics body
for (unsigned int k = 0; k < physicsManifoldsCount; k++)
{
if (contacts[k]->id == currentId)
{
currentId++;
break;
}
}
// If it is not used, use it as new physics body id
if (currentId == i)
{
index = i;
break;
}
}
return index;
}
// Creates a new physics manifold to solve collision
static PhysicsManifold CreatePhysicsManifold(PhysicsBody a, PhysicsBody b)
{
PhysicsManifold manifold = (PhysicsManifold)PHYSAC_MALLOC(sizeof(PhysicsManifoldData));
usedMemory += sizeof(PhysicsManifoldData);
int id = FindAvailableManifoldIndex();
if (id != -1)
{
// Initialize new manifold with generic values
manifold->id = id;
manifold->bodyA = a;
manifold->bodyB = b;
manifold->penetration = 0;
manifold->normal = PHYSAC_VECTOR_ZERO;
manifold->contacts[0] = PHYSAC_VECTOR_ZERO;
manifold->contacts[1] = PHYSAC_VECTOR_ZERO;
manifold->contactsCount = 0;
manifold->restitution = 0.0f;
manifold->dynamicFriction = 0.0f;
manifold->staticFriction = 0.0f;
// Add new body to bodies pointers array and update bodies count
contacts[physicsManifoldsCount] = manifold;
physicsManifoldsCount++;
}
else TRACELOG("[PHYSAC] Physic manifold could not be created, PHYSAC_MAX_MANIFOLDS reached\n");
return manifold;
}
// Unitializes and destroys a physics manifold
static void DestroyPhysicsManifold(PhysicsManifold manifold)
{
if (manifold != NULL)
{
int id = manifold->id;
int index = -1;
for (unsigned int i = 0; i < physicsManifoldsCount; i++)
{
if (contacts[i]->id == id)
{
index = i;
break;
}
}
if (index == -1) return; // Prevent access to index -1
// Free manifold allocated memory
PHYSAC_FREE(manifold);
usedMemory -= sizeof(PhysicsManifoldData);
contacts[index] = NULL;
// Reorder physics manifolds pointers array and its catched index
for (unsigned int i = index; i < physicsManifoldsCount; i++)
{
if ((i + 1) < physicsManifoldsCount) contacts[i] = contacts[i + 1];
}
// Update physics manifolds count
physicsManifoldsCount--;
}
else TRACELOG("[PHYSAC] WARNING: DestroyPhysicsManifold: NULL physic manifold\n");
}
// Solves a created physics manifold between two physics bodies
static void SolvePhysicsManifold(PhysicsManifold manifold)
{
switch (manifold->bodyA->shape.type)
{
case PHYSICS_CIRCLE:
{
switch (manifold->bodyB->shape.type)
{
case PHYSICS_CIRCLE: SolveCircleToCircle(manifold); break;
case PHYSICS_POLYGON: SolveCircleToPolygon(manifold); break;
default: break;
}
} break;
case PHYSICS_POLYGON:
{
switch (manifold->bodyB->shape.type)
{
case PHYSICS_CIRCLE: SolvePolygonToCircle(manifold); break;
case PHYSICS_POLYGON: SolvePolygonToPolygon(manifold); break;
default: break;
}
} break;
default: break;
}
// Update physics body grounded state if normal direction is down and grounded state is not set yet in previous manifolds
if (!manifold->bodyB->isGrounded) manifold->bodyB->isGrounded = (manifold->normal.y < 0);
}
// Solves collision between two circle shape physics bodies
static void SolveCircleToCircle(PhysicsManifold manifold)
{
PhysicsBody bodyA = manifold->bodyA;
PhysicsBody bodyB = manifold->bodyB;
if ((bodyA == NULL) || (bodyB == NULL)) return;
// Calculate translational vector, which is normal
Vector2 normal = MathVector2Subtract(bodyB->position, bodyA->position);
float distSqr = MathVector2SqrLen(normal);
float radius = bodyA->shape.radius + bodyB->shape.radius;
// Check if circles are not in contact
if (distSqr >= radius*radius)
{
manifold->contactsCount = 0;
return;
}
float distance = sqrtf(distSqr);
manifold->contactsCount = 1;
if (distance == 0.0f)
{
manifold->penetration = bodyA->shape.radius;
manifold->normal = CLITERAL(Vector2){ 1.0f, 0.0f };
manifold->contacts[0] = bodyA->position;
}
else
{
manifold->penetration = radius - distance;
manifold->normal = CLITERAL(Vector2){ normal.x/distance, normal.y/distance }; // Faster than using MathVector2Normalize() due to sqrt is already performed
manifold->contacts[0] = CLITERAL(Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y };
}
// Update physics body grounded state if normal direction is down
if (!bodyA->isGrounded) bodyA->isGrounded = (manifold->normal.y < 0);
}
// Solves collision between a circle to a polygon shape physics bodies
static void SolveCircleToPolygon(PhysicsManifold manifold)
{
PhysicsBody bodyA = manifold->bodyA;
PhysicsBody bodyB = manifold->bodyB;
if ((bodyA == NULL) || (bodyB == NULL)) return;
manifold->contactsCount = 0;
// Transform circle center to polygon transform space
Vector2 center = bodyA->position;
center = MathMatVector2Product(MathMatTranspose(bodyB->shape.transform), MathVector2Subtract(center, bodyB->position));
// Find edge with minimum penetration
// It is the same concept as using support points in SolvePolygonToPolygon
float separation = -PHYSAC_FLT_MAX;
int faceNormal = 0;
PhysicsVertexData vertexData = bodyB->shape.vertexData;
for (unsigned int i = 0; i < vertexData.vertexCount; i++)
{
float currentSeparation = MathVector2DotProduct(vertexData.normals[i], MathVector2Subtract(center, vertexData.positions[i]));
if (currentSeparation > bodyA->shape.radius) return;
if (currentSeparation > separation)
{
separation = currentSeparation;
faceNormal = i;
}
}
// Grab face's vertices
Vector2 v1 = vertexData.positions[faceNormal];
int nextIndex = (((faceNormal + 1) < (int)vertexData.vertexCount) ? (faceNormal + 1) : 0);
Vector2 v2 = vertexData.positions[nextIndex];
// Check to see if center is within polygon
if (separation < PHYSAC_EPSILON)
{
manifold->contactsCount = 1;
Vector2 normal = MathMatVector2Product(bodyB->shape.transform, vertexData.normals[faceNormal]);
manifold->normal = CLITERAL(Vector2){ -normal.x, -normal.y };
manifold->contacts[0] = CLITERAL(Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y };
manifold->penetration = bodyA->shape.radius;
return;
}
// Determine which voronoi region of the edge center of circle lies within
float dot1 = MathVector2DotProduct(MathVector2Subtract(center, v1), MathVector2Subtract(v2, v1));
float dot2 = MathVector2DotProduct(MathVector2Subtract(center, v2), MathVector2Subtract(v1, v2));
manifold->penetration = bodyA->shape.radius - separation;
if (dot1 <= 0.0f) // Closest to v1
{
if (MathVector2SqrDistance(center, v1) > bodyA->shape.radius*bodyA->shape.radius) return;
manifold->contactsCount = 1;
Vector2 normal = MathVector2Subtract(v1, center);
normal = MathMatVector2Product(bodyB->shape.transform, normal);
MathVector2Normalize(&normal);
manifold->normal = normal;
v1 = MathMatVector2Product(bodyB->shape.transform, v1);
v1 = MathVector2Add(v1, bodyB->position);
manifold->contacts[0] = v1;
}
else if (dot2 <= 0.0f) // Closest to v2
{
if (MathVector2SqrDistance(center, v2) > bodyA->shape.radius*bodyA->shape.radius) return;
manifold->contactsCount = 1;
Vector2 normal = MathVector2Subtract(v2, center);
v2 = MathMatVector2Product(bodyB->shape.transform, v2);
v2 = MathVector2Add(v2, bodyB->position);
manifold->contacts[0] = v2;
normal = MathMatVector2Product(bodyB->shape.transform, normal);
MathVector2Normalize(&normal);
manifold->normal = normal;
}
else // Closest to face
{
Vector2 normal = vertexData.normals[faceNormal];
if (MathVector2DotProduct(MathVector2Subtract(center, v1), normal) > bodyA->shape.radius) return;
normal = MathMatVector2Product(bodyB->shape.transform, normal);
manifold->normal = CLITERAL(Vector2){ -normal.x, -normal.y };
manifold->contacts[0] = CLITERAL(Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y };
manifold->contactsCount = 1;
}
}
// Solves collision between a polygon to a circle shape physics bodies
static void SolvePolygonToCircle(PhysicsManifold manifold)
{
PhysicsBody bodyA = manifold->bodyA;
PhysicsBody bodyB = manifold->bodyB;
if ((bodyA == NULL) || (bodyB == NULL)) return;
manifold->bodyA = bodyB;
manifold->bodyB = bodyA;
SolveCircleToPolygon(manifold);
manifold->normal.x *= -1.0f;
manifold->normal.y *= -1.0f;
}
// Solves collision between two polygons shape physics bodies
static void SolvePolygonToPolygon(PhysicsManifold manifold)
{
if ((manifold->bodyA == NULL) || (manifold->bodyB == NULL)) return;
PhysicsShape bodyA = manifold->bodyA->shape;
PhysicsShape bodyB = manifold->bodyB->shape;
manifold->contactsCount = 0;
// Check for separating axis with A shape's face planes
int faceA = 0;
float penetrationA = FindAxisLeastPenetration(&faceA, bodyA, bodyB);
if (penetrationA >= 0.0f) return;
// Check for separating axis with B shape's face planes
int faceB = 0;
float penetrationB = FindAxisLeastPenetration(&faceB, bodyB, bodyA);
if (penetrationB >= 0.0f) return;
int referenceIndex = 0;
bool flip = false; // Always point from A shape to B shape
PhysicsShape refPoly; // Reference
PhysicsShape incPoly; // Incident
// Determine which shape contains reference face
// Checking bias range for penetration
if (penetrationA >= (penetrationB*0.95f + penetrationA*0.01f))
{
refPoly = bodyA;
incPoly = bodyB;
referenceIndex = faceA;
}
else
{
refPoly = bodyB;
incPoly = bodyA;
referenceIndex = faceB;
flip = true;
}
// World space incident face
Vector2 incidentFace[2];
FindIncidentFace(&incidentFace[0], &incidentFace[1], refPoly, incPoly, referenceIndex);
// Setup reference face vertices
PhysicsVertexData refData = refPoly.vertexData;
Vector2 v1 = refData.positions[referenceIndex];
referenceIndex = (((referenceIndex + 1) < (int)refData.vertexCount) ? (referenceIndex + 1) : 0);
Vector2 v2 = refData.positions[referenceIndex];
// Transform vertices to world space
v1 = MathMatVector2Product(refPoly.transform, v1);
v1 = MathVector2Add(v1, refPoly.body->position);
v2 = MathMatVector2Product(refPoly.transform, v2);
v2 = MathVector2Add(v2, refPoly.body->position);
// Calculate reference face side normal in world space
Vector2 sidePlaneNormal = MathVector2Subtract(v2, v1);
MathVector2Normalize(&sidePlaneNormal);
// Orthogonalize
Vector2 refFaceNormal = { sidePlaneNormal.y, -sidePlaneNormal.x };
float refC = MathVector2DotProduct(refFaceNormal, v1);
float negSide = MathVector2DotProduct(sidePlaneNormal, v1)*-1;
float posSide = MathVector2DotProduct(sidePlaneNormal, v2);
// MathVector2Clip incident face to reference face side planes (due to floating point error, possible to not have required points
if (MathVector2Clip(CLITERAL(Vector2){ -sidePlaneNormal.x, -sidePlaneNormal.y }, &incidentFace[0], &incidentFace[1], negSide) < 2) return;
if (MathVector2Clip(sidePlaneNormal, &incidentFace[0], &incidentFace[1], posSide) < 2) return;
// Flip normal if required
manifold->normal = (flip ? CLITERAL(Vector2){ -refFaceNormal.x, -refFaceNormal.y } : refFaceNormal);
// Keep points behind reference face
int currentPoint = 0; // MathVector2Clipped points behind reference face
float separation = MathVector2DotProduct(refFaceNormal, incidentFace[0]) - refC;
if (separation <= 0.0f)
{
manifold->contacts[currentPoint] = incidentFace[0];
manifold->penetration = -separation;
currentPoint++;
}
else manifold->penetration = 0.0f;
separation = MathVector2DotProduct(refFaceNormal, incidentFace[1]) - refC;
if (separation <= 0.0f)
{
manifold->contacts[currentPoint] = incidentFace[1];
manifold->penetration += -separation;
currentPoint++;
// Calculate total penetration average
manifold->penetration /= currentPoint;
}
manifold->contactsCount = currentPoint;
}
// Integrates physics forces into velocity
static void IntegratePhysicsForces(PhysicsBody body)
{
if ((body == NULL) || (body->inverseMass == 0.0f) || !body->enabled) return;
body->velocity.x += (float)((body->force.x*body->inverseMass)*(deltaTime/2.0));
body->velocity.y += (float)((body->force.y*body->inverseMass)*(deltaTime/2.0));
if (body->useGravity)
{
body->velocity.x += (float)(gravityForce.x*(deltaTime/1000/2.0));
body->velocity.y += (float)(gravityForce.y*(deltaTime/1000/2.0));
}
if (!body->freezeOrient) body->angularVelocity += (float)(body->torque*body->inverseInertia*(deltaTime/2.0));
}
// Initializes physics manifolds to solve collisions
static void InitializePhysicsManifolds(PhysicsManifold manifold)
{
PhysicsBody bodyA = manifold->bodyA;
PhysicsBody bodyB = manifold->bodyB;
if ((bodyA == NULL) || (bodyB == NULL)) return;
// Calculate average restitution, static and dynamic friction
manifold->restitution = sqrtf(bodyA->restitution*bodyB->restitution);
manifold->staticFriction = sqrtf(bodyA->staticFriction*bodyB->staticFriction);
manifold->dynamicFriction = sqrtf(bodyA->dynamicFriction*bodyB->dynamicFriction);
for (unsigned int i = 0; i < manifold->contactsCount; i++)
{
// Caculate radius from center of mass to contact
Vector2 radiusA = MathVector2Subtract(manifold->contacts[i], bodyA->position);
Vector2 radiusB = MathVector2Subtract(manifold->contacts[i], bodyB->position);
Vector2 crossA = MathVector2Product(radiusA, bodyA->angularVelocity);
Vector2 crossB = MathVector2Product(radiusB, bodyB->angularVelocity);
Vector2 radiusV = { 0.0f, 0.0f };
radiusV.x = bodyB->velocity.x + crossB.x - bodyA->velocity.x - crossA.x;
radiusV.y = bodyB->velocity.y + crossB.y - bodyA->velocity.y - crossA.y;
// Determine if we should perform a resting collision or not;
// The idea is if the only thing moving this object is gravity, then the collision should be performed without any restitution
if (MathVector2SqrLen(radiusV) < (MathVector2SqrLen(CLITERAL(Vector2){ (float)(gravityForce.x*deltaTime/1000), (float)(gravityForce.y*deltaTime/1000) }) + PHYSAC_EPSILON)) manifold->restitution = 0;
}
}
// Integrates physics collisions impulses to solve collisions
static void IntegratePhysicsImpulses(PhysicsManifold manifold)
{
PhysicsBody bodyA = manifold->bodyA;
PhysicsBody bodyB = manifold->bodyB;
if ((bodyA == NULL) || (bodyB == NULL)) return;
// Early out and positional correct if both objects have infinite mass
if (fabs(bodyA->inverseMass + bodyB->inverseMass) <= PHYSAC_EPSILON)
{
bodyA->velocity = PHYSAC_VECTOR_ZERO;
bodyB->velocity = PHYSAC_VECTOR_ZERO;
return;
}
for (unsigned int i = 0; i < manifold->contactsCount; i++)
{
// Calculate radius from center of mass to contact
Vector2 radiusA = MathVector2Subtract(manifold->contacts[i], bodyA->position);
Vector2 radiusB = MathVector2Subtract(manifold->contacts[i], bodyB->position);
// Calculate relative velocity
Vector2 radiusV = { 0.0f, 0.0f };
radiusV.x = bodyB->velocity.x + MathVector2Product(radiusB, bodyB->angularVelocity).x - bodyA->velocity.x - MathVector2Product(radiusA, bodyA->angularVelocity).x;
radiusV.y = bodyB->velocity.y + MathVector2Product(radiusB, bodyB->angularVelocity).y - bodyA->velocity.y - MathVector2Product(radiusA, bodyA->angularVelocity).y;
// Relative velocity along the normal
float contactVelocity = MathVector2DotProduct(radiusV, manifold->normal);
// Do not resolve if velocities are separating
if (contactVelocity > 0.0f) return;
float raCrossN = MathVector2CrossProduct(radiusA, manifold->normal);
float rbCrossN = MathVector2CrossProduct(radiusB, manifold->normal);
float inverseMassSum = bodyA->inverseMass + bodyB->inverseMass + (raCrossN*raCrossN)*bodyA->inverseInertia + (rbCrossN*rbCrossN)*bodyB->inverseInertia;
// Calculate impulse scalar value
float impulse = -(1.0f + manifold->restitution)*contactVelocity;
impulse /= inverseMassSum;
impulse /= (float)manifold->contactsCount;
// Apply impulse to each physics body
Vector2 impulseV = { manifold->normal.x*impulse, manifold->normal.y*impulse };
if (bodyA->enabled)
{
bodyA->velocity.x += bodyA->inverseMass*(-impulseV.x);
bodyA->velocity.y += bodyA->inverseMass*(-impulseV.y);
if (!bodyA->freezeOrient) bodyA->angularVelocity += bodyA->inverseInertia*MathVector2CrossProduct(radiusA, CLITERAL(Vector2){ -impulseV.x, -impulseV.y });
}
if (bodyB->enabled)
{
bodyB->velocity.x += bodyB->inverseMass*(impulseV.x);
bodyB->velocity.y += bodyB->inverseMass*(impulseV.y);
if (!bodyB->freezeOrient) bodyB->angularVelocity += bodyB->inverseInertia*MathVector2CrossProduct(radiusB, impulseV);
}
// Apply friction impulse to each physics body
radiusV.x = bodyB->velocity.x + MathVector2Product(radiusB, bodyB->angularVelocity).x - bodyA->velocity.x - MathVector2Product(radiusA, bodyA->angularVelocity).x;
radiusV.y = bodyB->velocity.y + MathVector2Product(radiusB, bodyB->angularVelocity).y - bodyA->velocity.y - MathVector2Product(radiusA, bodyA->angularVelocity).y;
Vector2 tangent = { radiusV.x - (manifold->normal.x*MathVector2DotProduct(radiusV, manifold->normal)), radiusV.y - (manifold->normal.y*MathVector2DotProduct(radiusV, manifold->normal)) };
MathVector2Normalize(&tangent);
// Calculate impulse tangent magnitude
float impulseTangent = -MathVector2DotProduct(radiusV, tangent);
impulseTangent /= inverseMassSum;
impulseTangent /= (float)manifold->contactsCount;
float absImpulseTangent = (float)fabs(impulseTangent);
// Don't apply tiny friction impulses
if (absImpulseTangent <= PHYSAC_EPSILON) return;
// Apply coulumb's law
Vector2 tangentImpulse = { 0.0f, 0.0f };
if (absImpulseTangent < impulse*manifold->staticFriction) tangentImpulse = CLITERAL(Vector2){ tangent.x*impulseTangent, tangent.y*impulseTangent };
else tangentImpulse = CLITERAL(Vector2){ tangent.x*-impulse*manifold->dynamicFriction, tangent.y*-impulse*manifold->dynamicFriction };
// Apply friction impulse
if (bodyA->enabled)
{
bodyA->velocity.x += bodyA->inverseMass*(-tangentImpulse.x);
bodyA->velocity.y += bodyA->inverseMass*(-tangentImpulse.y);
if (!bodyA->freezeOrient) bodyA->angularVelocity += bodyA->inverseInertia*MathVector2CrossProduct(radiusA, CLITERAL(Vector2){ -tangentImpulse.x, -tangentImpulse.y });
}
if (bodyB->enabled)
{
bodyB->velocity.x += bodyB->inverseMass*(tangentImpulse.x);
bodyB->velocity.y += bodyB->inverseMass*(tangentImpulse.y);
if (!bodyB->freezeOrient) bodyB->angularVelocity += bodyB->inverseInertia*MathVector2CrossProduct(radiusB, tangentImpulse);
}
}
}
// Integrates physics velocity into position and forces
static void IntegratePhysicsVelocity(PhysicsBody body)
{
if ((body == NULL) ||!body->enabled) return;
body->position.x += (float)(body->velocity.x*deltaTime);
body->position.y += (float)(body->velocity.y*deltaTime);
if (!body->freezeOrient) body->orient += (float)(body->angularVelocity*deltaTime);
body->shape.transform = MathMatFromRadians(body->orient);
IntegratePhysicsForces(body);
}
// Corrects physics bodies positions based on manifolds collision information
static void CorrectPhysicsPositions(PhysicsManifold manifold)
{
PhysicsBody bodyA = manifold->bodyA;
PhysicsBody bodyB = manifold->bodyB;
if ((bodyA == NULL) || (bodyB == NULL)) return;
Vector2 correction = { 0.0f, 0.0f };
correction.x = (PHYSAC_MAX(manifold->penetration - PHYSAC_PENETRATION_ALLOWANCE, 0.0f)/(bodyA->inverseMass + bodyB->inverseMass))*manifold->normal.x*PHYSAC_PENETRATION_CORRECTION;
correction.y = (PHYSAC_MAX(manifold->penetration - PHYSAC_PENETRATION_ALLOWANCE, 0.0f)/(bodyA->inverseMass + bodyB->inverseMass))*manifold->normal.y*PHYSAC_PENETRATION_CORRECTION;
if (bodyA->enabled)
{
bodyA->position.x -= correction.x*bodyA->inverseMass;
bodyA->position.y -= correction.y*bodyA->inverseMass;
}
if (bodyB->enabled)
{
bodyB->position.x += correction.x*bodyB->inverseMass;
bodyB->position.y += correction.y*bodyB->inverseMass;
}
}
// Returns the extreme point along a direction within a polygon
static Vector2 GetSupport(PhysicsShape shape, Vector2 dir)
{
float bestProjection = -PHYSAC_FLT_MAX;
Vector2 bestVertex = { 0.0f, 0.0f };
PhysicsVertexData data = shape.vertexData;
for (unsigned int i = 0; i < data.vertexCount; i++)
{
Vector2 vertex = data.positions[i];
float projection = MathVector2DotProduct(vertex, dir);
if (projection > bestProjection)
{
bestVertex = vertex;
bestProjection = projection;
}
}
return bestVertex;
}
// Finds polygon shapes axis least penetration
static float FindAxisLeastPenetration(int *faceIndex, PhysicsShape shapeA, PhysicsShape shapeB)
{
float bestDistance = -PHYSAC_FLT_MAX;
int bestIndex = 0;
PhysicsVertexData dataA = shapeA.vertexData;
//PhysicsVertexData dataB = shapeB.vertexData;
for (unsigned int i = 0; i < dataA.vertexCount; i++)
{
// Retrieve a face normal from A shape
Vector2 normal = dataA.normals[i];
Vector2 transNormal = MathMatVector2Product(shapeA.transform, normal);
// Transform face normal into B shape's model space
Matrix2x2 buT = MathMatTranspose(shapeB.transform);
normal = MathMatVector2Product(buT, transNormal);
// Retrieve support point from B shape along -n
Vector2 support = GetSupport(shapeB, CLITERAL(Vector2){ -normal.x, -normal.y });
// Retrieve vertex on face from A shape, transform into B shape's model space
Vector2 vertex = dataA.positions[i];
vertex = MathMatVector2Product(shapeA.transform, vertex);
vertex = MathVector2Add(vertex, shapeA.body->position);
vertex = MathVector2Subtract(vertex, shapeB.body->position);
vertex = MathMatVector2Product(buT, vertex);
// Compute penetration distance in B shape's model space
float distance = MathVector2DotProduct(normal, MathVector2Subtract(support, vertex));
// Store greatest distance
if (distance > bestDistance)
{
bestDistance = distance;
bestIndex = i;
}
}
*faceIndex = bestIndex;
return bestDistance;
}
// Finds two polygon shapes incident face
static void FindIncidentFace(Vector2 *v0, Vector2 *v1, PhysicsShape ref, PhysicsShape inc, int index)
{
PhysicsVertexData refData = ref.vertexData;
PhysicsVertexData incData = inc.vertexData;
Vector2 referenceNormal = refData.normals[index];
// Calculate normal in incident's frame of reference
referenceNormal = MathMatVector2Product(ref.transform, referenceNormal); // To world space
referenceNormal = MathMatVector2Product(MathMatTranspose(inc.transform), referenceNormal); // To incident's model space
// Find most anti-normal face on polygon
int incidentFace = 0;
float minDot = PHYSAC_FLT_MAX;
for (unsigned int i = 0; i < incData.vertexCount; i++)
{
float dot = MathVector2DotProduct(referenceNormal, incData.normals[i]);
if (dot < minDot)
{
minDot = dot;
incidentFace = i;
}
}
// Assign face vertices for incident face
*v0 = MathMatVector2Product(inc.transform, incData.positions[incidentFace]);
*v0 = MathVector2Add(*v0, inc.body->position);
incidentFace = (((incidentFace + 1) < (int)incData.vertexCount) ? (incidentFace + 1) : 0);
*v1 = MathMatVector2Product(inc.transform, incData.positions[incidentFace]);
*v1 = MathVector2Add(*v1, inc.body->position);
}
// Returns clipping value based on a normal and two faces
static int MathVector2Clip(Vector2 normal, Vector2 *faceA, Vector2 *faceB, float clip)
{
int sp = 0;
Vector2 out[2] = { *faceA, *faceB };
// Retrieve distances from each endpoint to the line
float distanceA = MathVector2DotProduct(normal, *faceA) - clip;
float distanceB = MathVector2DotProduct(normal, *faceB) - clip;
// If negative (behind plane)
if (distanceA <= 0.0f) out[sp++] = *faceA;
if (distanceB <= 0.0f) out[sp++] = *faceB;
// If the points are on different sides of the plane
if ((distanceA*distanceB) < 0.0f)
{
// Push intersection point
float alpha = distanceA/(distanceA - distanceB);
out[sp] = *faceA;
Vector2 delta = MathVector2Subtract(*faceB, *faceA);
delta.x *= alpha;
delta.y *= alpha;
out[sp] = MathVector2Add(out[sp], delta);
sp++;
}
// Assign the new converted values
*faceA = out[0];
*faceB = out[1];
return sp;
}
// Returns the barycenter of a triangle given by 3 points
static Vector2 MathTriangleBarycenter(Vector2 v1, Vector2 v2, Vector2 v3)
{
Vector2 result = { 0.0f, 0.0f };
result.x = (v1.x + v2.x + v3.x)/3;
result.y = (v1.y + v2.y + v3.y)/3;
return result;
}
#if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
// Initializes hi-resolution MONOTONIC timer
static void InitTimer(void)
{
#if defined(_WIN32)
QueryPerformanceFrequency((unsigned long long int *) &frequency);
#endif
#if defined(__EMSCRIPTEN__) || defined(__linux__)
struct timespec now;
if (clock_gettime(CLOCK_MONOTONIC, &now) == 0) frequency = 1000000000;
#endif
#if defined(__APPLE__)
mach_timebase_info_data_t timebase;
mach_timebase_info(&timebase);
frequency = (timebase.denom*1e9)/timebase.numer;
#endif
baseClockTicks = (double)GetClockTicks(); // Get MONOTONIC clock time offset
startTime = GetCurrentTime(); // Get current time in milliseconds
}
// Get hi-res MONOTONIC time measure in clock ticks
static unsigned long long int GetClockTicks(void)
{
unsigned long long int value = 0;
#if defined(_WIN32)
QueryPerformanceCounter((unsigned long long int *) &value);
#endif
#if defined(__linux__)
struct timespec now;
clock_gettime(CLOCK_MONOTONIC, &now);
value = (unsigned long long int)now.tv_sec*(unsigned long long int)1000000000 + (unsigned long long int)now.tv_nsec;
#endif
#if defined(__APPLE__)
value = mach_absolute_time();
#endif
return value;
}
// Get current time in milliseconds
static double GetCurrentTime(void)
{
return (double)(GetClockTicks() - baseClockTicks)/frequency*1000;
}
#endif // !PHYSAC_AVOID_TIMMING_SYSTEM
// Returns the cross product of a vector and a value
static inline Vector2 MathVector2Product(Vector2 vector, float value)
{
Vector2 result = { -value*vector.y, value*vector.x };
return result;
}
// Returns the cross product of two vectors
static inline float MathVector2CrossProduct(Vector2 v1, Vector2 v2)
{
return (v1.x*v2.y - v1.y*v2.x);
}
// Returns the len square root of a vector
static inline float MathVector2SqrLen(Vector2 vector)
{
return (vector.x*vector.x + vector.y*vector.y);
}
// Returns the dot product of two vectors
static inline float MathVector2DotProduct(Vector2 v1, Vector2 v2)
{
return (v1.x*v2.x + v1.y*v2.y);
}
// Returns the square root of distance between two vectors
static inline float MathVector2SqrDistance(Vector2 v1, Vector2 v2)
{
Vector2 dir = MathVector2Subtract(v1, v2);
return MathVector2DotProduct(dir, dir);
}
// Returns the normalized values of a vector
static void MathVector2Normalize(Vector2 *vector)
{
float length, ilength;
Vector2 aux = *vector;
length = sqrtf(aux.x*aux.x + aux.y*aux.y);
if (length == 0) length = 1.0f;
ilength = 1.0f/length;
vector->x *= ilength;
vector->y *= ilength;
}
// Returns the sum of two given vectors
static inline Vector2 MathVector2Add(Vector2 v1, Vector2 v2)
{
Vector2 result = { v1.x + v2.x, v1.y + v2.y };
return result;
}
// Returns the subtract of two given vectors
static inline Vector2 MathVector2Subtract(Vector2 v1, Vector2 v2)
{
Vector2 result = { v1.x - v2.x, v1.y - v2.y };
return result;
}
// Creates a matrix 2x2 from a given radians value
static Matrix2x2 MathMatFromRadians(float radians)
{
float cos = cosf(radians);
float sin = sinf(radians);
Matrix2x2 result = { cos, -sin, sin, cos };
return result;
}
// Returns the transpose of a given matrix 2x2
static inline Matrix2x2 MathMatTranspose(Matrix2x2 matrix)
{
Matrix2x2 result = { matrix.m00, matrix.m10, matrix.m01, matrix.m11 };
return result;
}
// Multiplies a vector by a matrix 2x2
static inline Vector2 MathMatVector2Product(Matrix2x2 matrix, Vector2 vector)
{
Vector2 result = { matrix.m00*vector.x + matrix.m01*vector.y, matrix.m10*vector.x + matrix.m11*vector.y };
return result;
}
#endif // PHYSAC_IMPLEMENTATION
dpd/include/raudio.h 0 → 100644
View file @ b0598b21
/**********************************************************************************************
*
* raudio v1.0 - A simple and easy-to-use audio library based on miniaudio
*
* FEATURES:
* - Manage audio device (init/close)
* - Load and unload audio files
* - Format wave data (sample rate, size, channels)
* - Play/Stop/Pause/Resume loaded audio
* - Manage mixing channels
* - Manage raw audio context
*
* DEPENDENCIES:
* miniaudio.h - Audio device management lib (https://github.com/dr-soft/miniaudio)
* stb_vorbis.h - Ogg audio files loading (http://www.nothings.org/stb_vorbis/)
* dr_mp3.h - MP3 audio file loading (https://github.com/mackron/dr_libs)
* dr_flac.h - FLAC audio file loading (https://github.com/mackron/dr_libs)
* jar_xm.h - XM module file loading
* jar_mod.h - MOD audio file loading
*
* CONTRIBUTORS:
* David Reid (github: @mackron) (Nov. 2017):
* - Complete port to miniaudio library
*
* Joshua Reisenauer (github: @kd7tck) (2015)
* - XM audio module support (jar_xm)
* - MOD audio module support (jar_mod)
* - Mixing channels support
* - Raw audio context support
*
*
* LICENSE: zlib/libpng
*
* Copyright (c) 2014-2021 Ramon Santamaria (@raysan5)
*
* This software is provided "as-is", without any express or implied warranty. In no event
* will the authors be held liable for any damages arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose, including commercial
* applications, and to alter it and redistribute it freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not claim that you
* wrote the original software. If you use this software in a product, an acknowledgment
* in the product documentation would be appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be misrepresented
* as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*
**********************************************************************************************/
#ifndef RAUDIO_H
#define RAUDIO_H
//----------------------------------------------------------------------------------
// Defines and Macros
//----------------------------------------------------------------------------------
// Allow custom memory allocators
#ifndef RL_MALLOC
#define RL_MALLOC(sz) malloc(sz)
#endif
#ifndef RL_CALLOC
#define RL_CALLOC(n,sz) calloc(n,sz)
#endif
#ifndef RL_FREE
#define RL_FREE(p) free(p)
#endif
//----------------------------------------------------------------------------------
// Types and Structures Definition
//----------------------------------------------------------------------------------
#ifndef __cplusplus
// Boolean type
#if !defined(_STDBOOL_H)
typedef enum { false, true } bool;
#define _STDBOOL_H
#endif
#endif
// Wave type, defines audio wave data
typedef struct Wave {
unsigned int sampleCount; // Total number of samples
unsigned int sampleRate; // Frequency (samples per second)
unsigned int sampleSize; // Bit depth (bits per sample): 8, 16, 32 (24 not supported)
unsigned int channels; // Number of channels (1-mono, 2-stereo)
void *data; // Buffer data pointer
} Wave;
typedef struct rAudioBuffer rAudioBuffer;
// Audio stream type
// NOTE: Useful to create custom audio streams not bound to a specific file
typedef struct AudioStream {
unsigned int sampleRate; // Frequency (samples per second)
unsigned int sampleSize; // Bit depth (bits per sample): 8, 16, 32 (24 not supported)
unsigned int channels; // Number of channels (1-mono, 2-stereo)
rAudioBuffer *buffer; // Pointer to internal data used by the audio system
} AudioStream;
// Sound source type
typedef struct Sound {
unsigned int sampleCount; // Total number of samples
AudioStream stream; // Audio stream
} Sound;
// Music stream type (audio file streaming from memory)
// NOTE: Anything longer than ~10 seconds should be streamed
typedef struct Music {
int ctxType; // Type of music context (audio filetype)
void *ctxData; // Audio context data, depends on type
bool looping; // Music looping enable
unsigned int sampleCount; // Total number of samples
AudioStream stream; // Audio stream
} Music;
#ifdef __cplusplus
extern "C" { // Prevents name mangling of functions
#endif
//----------------------------------------------------------------------------------
// Global Variables Definition
//----------------------------------------------------------------------------------
//...
//----------------------------------------------------------------------------------
// Module Functions Declaration
//----------------------------------------------------------------------------------
// Audio device management functions
void InitAudioDevice(void); // Initialize audio device and context
void CloseAudioDevice(void); // Close the audio device and context
bool IsAudioDeviceReady(void); // Check if audio device has been initialized successfully
void SetMasterVolume(float volume); // Set master volume (listener)
// Wave/Sound loading/unloading functions
Wave LoadWave(const char *fileName); // Load wave data from file
Sound LoadSound(const char *fileName); // Load sound from file
Sound LoadSoundFromWave(Wave wave); // Load sound from wave data
void UpdateSound(Sound sound, const void *data, int samplesCount);// Update sound buffer with new data
void UnloadWave(Wave wave); // Unload wave data
void UnloadSound(Sound sound); // Unload sound
void ExportWave(Wave wave, const char *fileName); // Export wave data to file
void ExportWaveAsCode(Wave wave, const char *fileName); // Export wave sample data to code (.h)
// Wave/Sound management functions
void PlaySound(Sound sound); // Play a sound
void StopSound(Sound sound); // Stop playing a sound
void PauseSound(Sound sound); // Pause a sound
void ResumeSound(Sound sound); // Resume a paused sound
void PlaySoundMulti(Sound sound); // Play a sound (using multichannel buffer pool)
void StopSoundMulti(void); // Stop any sound playing (using multichannel buffer pool)
int GetSoundsPlaying(void); // Get number of sounds playing in the multichannel
bool IsSoundPlaying(Sound sound); // Check if a sound is currently playing
void SetSoundVolume(Sound sound, float volume); // Set volume for a sound (1.0 is max level)
void SetSoundPitch(Sound sound, float pitch); // Set pitch for a sound (1.0 is base level)
void WaveFormat(Wave *wave, int sampleRate, int sampleSize, int channels); // Convert wave data to desired format
Wave WaveCopy(Wave wave); // Copy a wave to a new wave
void WaveCrop(Wave *wave, int initSample, int finalSample); // Crop a wave to defined samples range
float *GetWaveData(Wave wave); // Get samples data from wave as a floats array
// Music management functions
Music LoadMusicStream(const char *fileName); // Load music stream from file
Music LoadMusicStreamFromMemory(const char *fileType, unsigned char* data, int dataSize); // Load module music from data
void UnloadMusicStream(Music music); // Unload music stream
void PlayMusicStream(Music music); // Start music playing
void UpdateMusicStream(Music music); // Updates buffers for music streaming
void StopMusicStream(Music music); // Stop music playing
void PauseMusicStream(Music music); // Pause music playing
void ResumeMusicStream(Music music); // Resume playing paused music
bool IsMusicPlaying(Music music); // Check if music is playing
void SetMusicVolume(Music music, float volume); // Set volume for music (1.0 is max level)
void SetMusicPitch(Music music, float pitch); // Set pitch for a music (1.0 is base level)
float GetMusicTimeLength(Music music); // Get music time length (in seconds)
float GetMusicTimePlayed(Music music); // Get current music time played (in seconds)
// AudioStream management functions
AudioStream InitAudioStream(unsigned int sampleRate, unsigned int sampleSize, unsigned int channels); // Init audio stream (to stream raw audio pcm data)
void UpdateAudioStream(AudioStream stream, const void *data, int samplesCount); // Update audio stream buffers with data
void CloseAudioStream(AudioStream stream); // Close audio stream and free memory
bool IsAudioStreamProcessed(AudioStream stream); // Check if any audio stream buffers requires refill
void PlayAudioStream(AudioStream stream); // Play audio stream
void PauseAudioStream(AudioStream stream); // Pause audio stream
void ResumeAudioStream(AudioStream stream); // Resume audio stream
bool IsAudioStreamPlaying(AudioStream stream); // Check if audio stream is playing
void StopAudioStream(AudioStream stream); // Stop audio stream
void SetAudioStreamVolume(AudioStream stream, float volume); // Set volume for audio stream (1.0 is max level)
void SetAudioStreamPitch(AudioStream stream, float pitch); // Set pitch for audio stream (1.0 is base level)
void SetAudioStreamBufferSizeDefault(int size); // Default size for new audio streams
#ifdef __cplusplus
}
#endif
#endif // RAUDIO_H
dpd/include/raylib.h 0 → 100644
View file @ b0598b21
This source diff could not be displayed because it is too large. You can view the blob instead.
dpd/include/raymath.h 0 → 100644
View file @ b0598b21
/**********************************************************************************************
*
* raymath v1.2 - Math functions to work with Vector3, Matrix and Quaternions
*
* CONFIGURATION:
*
* #define RAYMATH_IMPLEMENTATION
* Generates the implementation of the library into the included file.
* If not defined, the library is in header only mode and can be included in other headers
* or source files without problems. But only ONE file should hold the implementation.
*
* #define RAYMATH_HEADER_ONLY
* Define static inline functions code, so #include header suffices for use.
* This may use up lots of memory.
*
* #define RAYMATH_STANDALONE
* Avoid raylib.h header inclusion in this file.
* Vector3 and Matrix data types are defined internally in raymath module.
*
*
* LICENSE: zlib/libpng
*
* Copyright (c) 2015-2021 Ramon Santamaria (@raysan5)
*
* This software is provided "as-is", without any express or implied warranty. In no event
* will the authors be held liable for any damages arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose, including commercial
* applications, and to alter it and redistribute it freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not claim that you
* wrote the original software. If you use this software in a product, an acknowledgment
* in the product documentation would be appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be misrepresented
* as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*
**********************************************************************************************/
#ifndef RAYMATH_H
#define RAYMATH_H
//#define RAYMATH_STANDALONE // NOTE: To use raymath as standalone lib, just uncomment this line
//#define RAYMATH_HEADER_ONLY // NOTE: To compile functions as static inline, uncomment this line
#ifndef RAYMATH_STANDALONE
#include "raylib.h" // Required for structs: Vector3, Matrix
#endif
#if defined(RAYMATH_IMPLEMENTATION) && defined(RAYMATH_HEADER_ONLY)
#error "Specifying both RAYMATH_IMPLEMENTATION and RAYMATH_HEADER_ONLY is contradictory"
#endif
#if defined(RAYMATH_IMPLEMENTATION)
#if defined(_WIN32) && defined(BUILD_LIBTYPE_SHARED)
#define RMDEF __declspec(dllexport) extern inline // We are building raylib as a Win32 shared library (.dll).
#elif defined(_WIN32) && defined(USE_LIBTYPE_SHARED)
#define RMDEF __declspec(dllimport) // We are using raylib as a Win32 shared library (.dll)
#else
#define RMDEF extern inline // Provide external definition
#endif
#elif defined(RAYMATH_HEADER_ONLY)
#define RMDEF static inline // Functions may be inlined, no external out-of-line definition
#else
#if defined(__TINYC__)
#define RMDEF static inline // plain inline not supported by tinycc (See issue #435)
#else
#define RMDEF inline // Functions may be inlined or external definition used
#endif
#endif
//----------------------------------------------------------------------------------
// Defines and Macros
//----------------------------------------------------------------------------------
#ifndef PI
#define PI 3.14159265358979323846f
#endif
#ifndef DEG2RAD
#define DEG2RAD (PI/180.0f)
#endif
#ifndef RAD2DEG
#define RAD2DEG (180.0f/PI)
#endif
// Return float vector for Matrix
#ifndef MatrixToFloat
#define MatrixToFloat(mat) (MatrixToFloatV(mat).v)
#endif
// Return float vector for Vector3
#ifndef Vector3ToFloat
#define Vector3ToFloat(vec) (Vector3ToFloatV(vec).v)
#endif
//----------------------------------------------------------------------------------
// Types and Structures Definition
//----------------------------------------------------------------------------------
#if defined(RAYMATH_STANDALONE)
// Vector2 type
typedef struct Vector2 {
float x;
float y;
} Vector2;
// Vector3 type
typedef struct Vector3 {
float x;
float y;
float z;
} Vector3;
// Vector4 type
typedef struct Vector4 {
float x;
float y;
float z;
float w;
} Vector4;
// Quaternion type
typedef Vector4 Quaternion;
// Matrix type (OpenGL style 4x4 - right handed, column major)
typedef struct Matrix {
float m0, m4, m8, m12;
float m1, m5, m9, m13;
float m2, m6, m10, m14;
float m3, m7, m11, m15;
} Matrix;
#endif
// NOTE: Helper types to be used instead of array return types for *ToFloat functions
typedef struct float3 { float v[3]; } float3;
typedef struct float16 { float v[16]; } float16;
#include <math.h> // Required for: sinf(), cosf(), sqrtf(), tan(), fabs()
//----------------------------------------------------------------------------------
// Module Functions Definition - Utils math
//----------------------------------------------------------------------------------
// Clamp float value
RMDEF float Clamp(float value, float min, float max)
{
const float res = value < min ? min : value;
return res > max ? max : res;
}
// Calculate linear interpolation between two floats
RMDEF float Lerp(float start, float end, float amount)
{
return start + amount*(end - start);
}
// Normalize input value within input range
RMDEF float Normalize(float value, float start, float end)
{
return (value - start) / (end - start);
}
// Remap input value within input range to output range
RMDEF float Remap(float value, float inputStart, float inputEnd, float outputStart, float outputEnd)
{
return (value - inputStart) / (inputEnd - inputStart) * (outputEnd - outputStart) + outputStart;
}
//----------------------------------------------------------------------------------
// Module Functions Definition - Vector2 math
//----------------------------------------------------------------------------------
// Vector with components value 0.0f
RMDEF Vector2 Vector2Zero(void)
{
Vector2 result = { 0.0f, 0.0f };
return result;
}
// Vector with components value 1.0f
RMDEF Vector2 Vector2One(void)
{
Vector2 result = { 1.0f, 1.0f };
return result;
}
// Add two vectors (v1 + v2)
RMDEF Vector2 Vector2Add(Vector2 v1, Vector2 v2)
{
Vector2 result = { v1.x + v2.x, v1.y + v2.y };
return result;
}
// Add vector and float value
RMDEF Vector2 Vector2AddValue(Vector2 v, float add)
{
Vector2 result = { v.x + add, v.y + add };
return result;
}
// Subtract two vectors (v1 - v2)
RMDEF Vector2 Vector2Subtract(Vector2 v1, Vector2 v2)
{
Vector2 result = { v1.x - v2.x, v1.y - v2.y };
return result;
}
// Subtract vector by float value
RMDEF Vector2 Vector2SubtractValue(Vector2 v, float sub)
{
Vector2 result = { v.x - sub, v.y - sub };
return result;
}
// Calculate vector length
RMDEF float Vector2Length(Vector2 v)
{
float result = sqrtf((v.x*v.x) + (v.y*v.y));
return result;
}
// Calculate vector square length
RMDEF float Vector2LengthSqr(Vector2 v)
{
float result = (v.x*v.x) + (v.y*v.y);
return result;
}
// Calculate two vectors dot product
RMDEF float Vector2DotProduct(Vector2 v1, Vector2 v2)
{
float result = (v1.x*v2.x + v1.y*v2.y);
return result;
}
// Calculate distance between two vectors
RMDEF float Vector2Distance(Vector2 v1, Vector2 v2)
{
float result = sqrtf((v1.x - v2.x)*(v1.x - v2.x) + (v1.y - v2.y)*(v1.y - v2.y));
return result;
}
// Calculate angle from two vectors in X-axis
RMDEF float Vector2Angle(Vector2 v1, Vector2 v2)
{
float result = atan2f(v2.y - v1.y, v2.x - v1.x)*(180.0f/PI);
if (result < 0) result += 360.0f;
return result;
}
// Scale vector (multiply by value)
RMDEF Vector2 Vector2Scale(Vector2 v, float scale)
{
Vector2 result = { v.x*scale, v.y*scale };
return result;
}
// Multiply vector by vector
RMDEF Vector2 Vector2Multiply(Vector2 v1, Vector2 v2)
{
Vector2 result = { v1.x*v2.x, v1.y*v2.y };
return result;
}
// Negate vector
RMDEF Vector2 Vector2Negate(Vector2 v)
{
Vector2 result = { -v.x, -v.y };
return result;
}
// Divide vector by vector
RMDEF Vector2 Vector2Divide(Vector2 v1, Vector2 v2)
{
Vector2 result = { v1.x/v2.x, v1.y/v2.y };
return result;
}
// Normalize provided vector
RMDEF Vector2 Vector2Normalize(Vector2 v)
{
Vector2 result = Vector2Scale(v, 1/Vector2Length(v));
return result;
}
// Calculate linear interpolation between two vectors
RMDEF Vector2 Vector2Lerp(Vector2 v1, Vector2 v2, float amount)
{
Vector2 result = { 0 };
result.x = v1.x + amount*(v2.x - v1.x);
result.y = v1.y + amount*(v2.y - v1.y);
return result;
}
// Calculate reflected vector to normal
RMDEF Vector2 Vector2Reflect(Vector2 v, Vector2 normal)
{
Vector2 result = { 0 };
float dotProduct = Vector2DotProduct(v, normal);
result.x = v.x - (2.0f*normal.x)*dotProduct;
result.y = v.y - (2.0f*normal.y)*dotProduct;
return result;
}
// Rotate Vector by float in Degrees.
RMDEF Vector2 Vector2Rotate(Vector2 v, float degs)
{
float rads = degs*DEG2RAD;
Vector2 result = {v.x * cosf(rads) - v.y * sinf(rads) , v.x * sinf(rads) + v.y * cosf(rads) };
return result;
}
// Move Vector towards target
RMDEF Vector2 Vector2MoveTowards(Vector2 v, Vector2 target, float maxDistance)
{
Vector2 result = { 0 };
float dx = target.x - v.x;
float dy = target.y - v.y;
float value = (dx*dx) + (dy*dy);
if ((value == 0) || ((maxDistance >= 0) && (value <= maxDistance*maxDistance))) result = target;
float dist = sqrtf(value);
result.x = v.x + dx/dist*maxDistance;
result.y = v.y + dy/dist*maxDistance;
return result;
}
//----------------------------------------------------------------------------------
// Module Functions Definition - Vector3 math
//----------------------------------------------------------------------------------
// Vector with components value 0.0f
RMDEF Vector3 Vector3Zero(void)
{
Vector3 result = { 0.0f, 0.0f, 0.0f };
return result;
}
// Vector with components value 1.0f
RMDEF Vector3 Vector3One(void)
{
Vector3 result = { 1.0f, 1.0f, 1.0f };
return result;
}
// Add two vectors
RMDEF Vector3 Vector3Add(Vector3 v1, Vector3 v2)
{
Vector3 result = { v1.x + v2.x, v1.y + v2.y, v1.z + v2.z };
return result;
}
// Add vector and float value
RMDEF Vector3 Vector3AddValue(Vector3 v, float add)
{
Vector3 result = { v.x + add, v.y + add, v.z + add };
return result;
}
// Subtract two vectors
RMDEF Vector3 Vector3Subtract(Vector3 v1, Vector3 v2)
{
Vector3 result = { v1.x - v2.x, v1.y - v2.y, v1.z - v2.z };
return result;
}
// Subtract vector by float value
RMDEF Vector3 Vector3SubtractValue(Vector3 v, float sub)
{
Vector3 result = { v.x - sub, v.y - sub, v.z - sub };
return result;
}
// Multiply vector by scalar
RMDEF Vector3 Vector3Scale(Vector3 v, float scalar)
{
Vector3 result = { v.x*scalar, v.y*scalar, v.z*scalar };
return result;
}
// Multiply vector by vector
RMDEF Vector3 Vector3Multiply(Vector3 v1, Vector3 v2)
{
Vector3 result = { v1.x*v2.x, v1.y*v2.y, v1.z*v2.z };
return result;
}
// Calculate two vectors cross product
RMDEF Vector3 Vector3CrossProduct(Vector3 v1, Vector3 v2)
{
Vector3 result = { v1.y*v2.z - v1.z*v2.y, v1.z*v2.x - v1.x*v2.z, v1.x*v2.y - v1.y*v2.x };
return result;
}
// Calculate one vector perpendicular vector
RMDEF Vector3 Vector3Perpendicular(Vector3 v)
{
Vector3 result = { 0 };
float min = (float) fabs(v.x);
Vector3 cardinalAxis = {1.0f, 0.0f, 0.0f};
if (fabs(v.y) < min)
{
min = (float) fabs(v.y);
Vector3 tmp = {0.0f, 1.0f, 0.0f};
cardinalAxis = tmp;
}
if (fabs(v.z) < min)
{
Vector3 tmp = {0.0f, 0.0f, 1.0f};
cardinalAxis = tmp;
}
result = Vector3CrossProduct(v, cardinalAxis);
return result;
}
// Calculate vector length
RMDEF float Vector3Length(const Vector3 v)
{
float result = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
return result;
}
// Calculate vector square length
RMDEF float Vector3LengthSqr(const Vector3 v)
{
float result = v.x*v.x + v.y*v.y + v.z*v.z;
return result;
}
// Calculate two vectors dot product
RMDEF float Vector3DotProduct(Vector3 v1, Vector3 v2)
{
float result = (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z);
return result;
}
// Calculate distance between two vectors
RMDEF float Vector3Distance(Vector3 v1, Vector3 v2)
{
float dx = v2.x - v1.x;
float dy = v2.y - v1.y;
float dz = v2.z - v1.z;
float result = sqrtf(dx*dx + dy*dy + dz*dz);
return result;
}
// Negate provided vector (invert direction)
RMDEF Vector3 Vector3Negate(Vector3 v)
{
Vector3 result = { -v.x, -v.y, -v.z };
return result;
}
// Divide vector by vector
RMDEF Vector3 Vector3Divide(Vector3 v1, Vector3 v2)
{
Vector3 result = { v1.x/v2.x, v1.y/v2.y, v1.z/v2.z };
return result;
}
// Normalize provided vector
RMDEF Vector3 Vector3Normalize(Vector3 v)
{
Vector3 result = v;
float length, ilength;
length = Vector3Length(v);
if (length == 0.0f) length = 1.0f;
ilength = 1.0f/length;
result.x *= ilength;
result.y *= ilength;
result.z *= ilength;
return result;
}
// Orthonormalize provided vectors
// Makes vectors normalized and orthogonal to each other
// Gram-Schmidt function implementation
RMDEF void Vector3OrthoNormalize(Vector3 *v1, Vector3 *v2)
{
*v1 = Vector3Normalize(*v1);
Vector3 vn = Vector3CrossProduct(*v1, *v2);
vn = Vector3Normalize(vn);
*v2 = Vector3CrossProduct(vn, *v1);
}
// Transforms a Vector3 by a given Matrix
RMDEF Vector3 Vector3Transform(Vector3 v, Matrix mat)
{
Vector3 result = { 0 };
float x = v.x;
float y = v.y;
float z = v.z;
result.x = mat.m0*x + mat.m4*y + mat.m8*z + mat.m12;
result.y = mat.m1*x + mat.m5*y + mat.m9*z + mat.m13;
result.z = mat.m2*x + mat.m6*y + mat.m10*z + mat.m14;
return result;
}
// Transform a vector by quaternion rotation
RMDEF Vector3 Vector3RotateByQuaternion(Vector3 v, Quaternion q)
{
Vector3 result = { 0 };
result.x = v.x*(q.x*q.x + q.w*q.w - q.y*q.y - q.z*q.z) + v.y*(2*q.x*q.y - 2*q.w*q.z) + v.z*(2*q.x*q.z + 2*q.w*q.y);
result.y = v.x*(2*q.w*q.z + 2*q.x*q.y) + v.y*(q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z) + v.z*(-2*q.w*q.x + 2*q.y*q.z);
result.z = v.x*(-2*q.w*q.y + 2*q.x*q.z) + v.y*(2*q.w*q.x + 2*q.y*q.z)+ v.z*(q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
return result;
}
// Calculate linear interpolation between two vectors
RMDEF Vector3 Vector3Lerp(Vector3 v1, Vector3 v2, float amount)
{
Vector3 result = { 0 };
result.x = v1.x + amount*(v2.x - v1.x);
result.y = v1.y + amount*(v2.y - v1.y);
result.z = v1.z + amount*(v2.z - v1.z);
return result;
}
// Calculate reflected vector to normal
RMDEF Vector3 Vector3Reflect(Vector3 v, Vector3 normal)
{
// I is the original vector
// N is the normal of the incident plane
// R = I - (2*N*( DotProduct[ I,N] ))
Vector3 result = { 0 };
float dotProduct = Vector3DotProduct(v, normal);
result.x = v.x - (2.0f*normal.x)*dotProduct;
result.y = v.y - (2.0f*normal.y)*dotProduct;
result.z = v.z - (2.0f*normal.z)*dotProduct;
return result;
}
// Return min value for each pair of components
RMDEF Vector3 Vector3Min(Vector3 v1, Vector3 v2)
{
Vector3 result = { 0 };
result.x = fminf(v1.x, v2.x);
result.y = fminf(v1.y, v2.y);
result.z = fminf(v1.z, v2.z);
return result;
}
// Return max value for each pair of components
RMDEF Vector3 Vector3Max(Vector3 v1, Vector3 v2)
{
Vector3 result = { 0 };
result.x = fmaxf(v1.x, v2.x);
result.y = fmaxf(v1.y, v2.y);
result.z = fmaxf(v1.z, v2.z);
return result;
}
// Compute barycenter coordinates (u, v, w) for point p with respect to triangle (a, b, c)
// NOTE: Assumes P is on the plane of the triangle
RMDEF Vector3 Vector3Barycenter(Vector3 p, Vector3 a, Vector3 b, Vector3 c)
{
//Vector v0 = b - a, v1 = c - a, v2 = p - a;
Vector3 v0 = Vector3Subtract(b, a);
Vector3 v1 = Vector3Subtract(c, a);
Vector3 v2 = Vector3Subtract(p, a);
float d00 = Vector3DotProduct(v0, v0);
float d01 = Vector3DotProduct(v0, v1);
float d11 = Vector3DotProduct(v1, v1);
float d20 = Vector3DotProduct(v2, v0);
float d21 = Vector3DotProduct(v2, v1);
float denom = d00*d11 - d01*d01;
Vector3 result = { 0 };
result.y = (d11*d20 - d01*d21)/denom;
result.z = (d00*d21 - d01*d20)/denom;
result.x = 1.0f - (result.z + result.y);
return result;
}
// Returns Vector3 as float array
RMDEF float3 Vector3ToFloatV(Vector3 v)
{
float3 buffer = { 0 };
buffer.v[0] = v.x;
buffer.v[1] = v.y;
buffer.v[2] = v.z;
return buffer;
}
//----------------------------------------------------------------------------------
// Module Functions Definition - Matrix math
//----------------------------------------------------------------------------------
// Compute matrix determinant
RMDEF float MatrixDeterminant(Matrix mat)
{
// Cache the matrix values (speed optimization)
float a00 = mat.m0, a01 = mat.m1, a02 = mat.m2, a03 = mat.m3;
float a10 = mat.m4, a11 = mat.m5, a12 = mat.m6, a13 = mat.m7;
float a20 = mat.m8, a21 = mat.m9, a22 = mat.m10, a23 = mat.m11;
float a30 = mat.m12, a31 = mat.m13, a32 = mat.m14, a33 = mat.m15;
float result = a30*a21*a12*a03 - a20*a31*a12*a03 - a30*a11*a22*a03 + a10*a31*a22*a03 +
a20*a11*a32*a03 - a10*a21*a32*a03 - a30*a21*a02*a13 + a20*a31*a02*a13 +
a30*a01*a22*a13 - a00*a31*a22*a13 - a20*a01*a32*a13 + a00*a21*a32*a13 +
a30*a11*a02*a23 - a10*a31*a02*a23 - a30*a01*a12*a23 + a00*a31*a12*a23 +
a10*a01*a32*a23 - a00*a11*a32*a23 - a20*a11*a02*a33 + a10*a21*a02*a33 +
a20*a01*a12*a33 - a00*a21*a12*a33 - a10*a01*a22*a33 + a00*a11*a22*a33;
return result;
}
// Returns the trace of the matrix (sum of the values along the diagonal)
RMDEF float MatrixTrace(Matrix mat)
{
float result = (mat.m0 + mat.m5 + mat.m10 + mat.m15);
return result;
}
// Transposes provided matrix
RMDEF Matrix MatrixTranspose(Matrix mat)
{
Matrix result = { 0 };
result.m0 = mat.m0;
result.m1 = mat.m4;
result.m2 = mat.m8;
result.m3 = mat.m12;
result.m4 = mat.m1;
result.m5 = mat.m5;
result.m6 = mat.m9;
result.m7 = mat.m13;
result.m8 = mat.m2;
result.m9 = mat.m6;
result.m10 = mat.m10;
result.m11 = mat.m14;
result.m12 = mat.m3;
result.m13 = mat.m7;
result.m14 = mat.m11;
result.m15 = mat.m15;
return result;
}
// Invert provided matrix
RMDEF Matrix MatrixInvert(Matrix mat)
{
Matrix result = { 0 };
// Cache the matrix values (speed optimization)
float a00 = mat.m0, a01 = mat.m1, a02 = mat.m2, a03 = mat.m3;
float a10 = mat.m4, a11 = mat.m5, a12 = mat.m6, a13 = mat.m7;
float a20 = mat.m8, a21 = mat.m9, a22 = mat.m10, a23 = mat.m11;
float a30 = mat.m12, a31 = mat.m13, a32 = mat.m14, a33 = mat.m15;
float b00 = a00*a11 - a01*a10;
float b01 = a00*a12 - a02*a10;
float b02 = a00*a13 - a03*a10;
float b03 = a01*a12 - a02*a11;
float b04 = a01*a13 - a03*a11;
float b05 = a02*a13 - a03*a12;
float b06 = a20*a31 - a21*a30;
float b07 = a20*a32 - a22*a30;
float b08 = a20*a33 - a23*a30;
float b09 = a21*a32 - a22*a31;
float b10 = a21*a33 - a23*a31;
float b11 = a22*a33 - a23*a32;
// Calculate the invert determinant (inlined to avoid double-caching)
float invDet = 1.0f/(b00*b11 - b01*b10 + b02*b09 + b03*b08 - b04*b07 + b05*b06);
result.m0 = (a11*b11 - a12*b10 + a13*b09)*invDet;
result.m1 = (-a01*b11 + a02*b10 - a03*b09)*invDet;
result.m2 = (a31*b05 - a32*b04 + a33*b03)*invDet;
result.m3 = (-a21*b05 + a22*b04 - a23*b03)*invDet;
result.m4 = (-a10*b11 + a12*b08 - a13*b07)*invDet;
result.m5 = (a00*b11 - a02*b08 + a03*b07)*invDet;
result.m6 = (-a30*b05 + a32*b02 - a33*b01)*invDet;
result.m7 = (a20*b05 - a22*b02 + a23*b01)*invDet;
result.m8 = (a10*b10 - a11*b08 + a13*b06)*invDet;
result.m9 = (-a00*b10 + a01*b08 - a03*b06)*invDet;
result.m10 = (a30*b04 - a31*b02 + a33*b00)*invDet;
result.m11 = (-a20*b04 + a21*b02 - a23*b00)*invDet;
result.m12 = (-a10*b09 + a11*b07 - a12*b06)*invDet;
result.m13 = (a00*b09 - a01*b07 + a02*b06)*invDet;
result.m14 = (-a30*b03 + a31*b01 - a32*b00)*invDet;
result.m15 = (a20*b03 - a21*b01 + a22*b00)*invDet;
return result;
}
// Normalize provided matrix
RMDEF Matrix MatrixNormalize(Matrix mat)
{
Matrix result = { 0 };
float det = MatrixDeterminant(mat);
result.m0 = mat.m0/det;
result.m1 = mat.m1/det;
result.m2 = mat.m2/det;
result.m3 = mat.m3/det;
result.m4 = mat.m4/det;
result.m5 = mat.m5/det;
result.m6 = mat.m6/det;
result.m7 = mat.m7/det;
result.m8 = mat.m8/det;
result.m9 = mat.m9/det;
result.m10 = mat.m10/det;
result.m11 = mat.m11/det;
result.m12 = mat.m12/det;
result.m13 = mat.m13/det;
result.m14 = mat.m14/det;
result.m15 = mat.m15/det;
return result;
}
// Returns identity matrix
RMDEF Matrix MatrixIdentity(void)
{
Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f };
return result;
}
// Add two matrices
RMDEF Matrix MatrixAdd(Matrix left, Matrix right)
{
Matrix result = MatrixIdentity();
result.m0 = left.m0 + right.m0;
result.m1 = left.m1 + right.m1;
result.m2 = left.m2 + right.m2;
result.m3 = left.m3 + right.m3;
result.m4 = left.m4 + right.m4;
result.m5 = left.m5 + right.m5;
result.m6 = left.m6 + right.m6;
result.m7 = left.m7 + right.m7;
result.m8 = left.m8 + right.m8;
result.m9 = left.m9 + right.m9;
result.m10 = left.m10 + right.m10;
result.m11 = left.m11 + right.m11;
result.m12 = left.m12 + right.m12;
result.m13 = left.m13 + right.m13;
result.m14 = left.m14 + right.m14;
result.m15 = left.m15 + right.m15;
return result;
}
// Subtract two matrices (left - right)
RMDEF Matrix MatrixSubtract(Matrix left, Matrix right)
{
Matrix result = MatrixIdentity();
result.m0 = left.m0 - right.m0;
result.m1 = left.m1 - right.m1;
result.m2 = left.m2 - right.m2;
result.m3 = left.m3 - right.m3;
result.m4 = left.m4 - right.m4;
result.m5 = left.m5 - right.m5;
result.m6 = left.m6 - right.m6;
result.m7 = left.m7 - right.m7;
result.m8 = left.m8 - right.m8;
result.m9 = left.m9 - right.m9;
result.m10 = left.m10 - right.m10;
result.m11 = left.m11 - right.m11;
result.m12 = left.m12 - right.m12;
result.m13 = left.m13 - right.m13;
result.m14 = left.m14 - right.m14;
result.m15 = left.m15 - right.m15;
return result;
}
// Returns two matrix multiplication
// NOTE: When multiplying matrices... the order matters!
RMDEF Matrix MatrixMultiply(Matrix left, Matrix right)
{
Matrix result = { 0 };
result.m0 = left.m0*right.m0 + left.m1*right.m4 + left.m2*right.m8 + left.m3*right.m12;
result.m1 = left.m0*right.m1 + left.m1*right.m5 + left.m2*right.m9 + left.m3*right.m13;
result.m2 = left.m0*right.m2 + left.m1*right.m6 + left.m2*right.m10 + left.m3*right.m14;
result.m3 = left.m0*right.m3 + left.m1*right.m7 + left.m2*right.m11 + left.m3*right.m15;
result.m4 = left.m4*right.m0 + left.m5*right.m4 + left.m6*right.m8 + left.m7*right.m12;
result.m5 = left.m4*right.m1 + left.m5*right.m5 + left.m6*right.m9 + left.m7*right.m13;
result.m6 = left.m4*right.m2 + left.m5*right.m6 + left.m6*right.m10 + left.m7*right.m14;
result.m7 = left.m4*right.m3 + left.m5*right.m7 + left.m6*right.m11 + left.m7*right.m15;
result.m8 = left.m8*right.m0 + left.m9*right.m4 + left.m10*right.m8 + left.m11*right.m12;
result.m9 = left.m8*right.m1 + left.m9*right.m5 + left.m10*right.m9 + left.m11*right.m13;
result.m10 = left.m8*right.m2 + left.m9*right.m6 + left.m10*right.m10 + left.m11*right.m14;
result.m11 = left.m8*right.m3 + left.m9*right.m7 + left.m10*right.m11 + left.m11*right.m15;
result.m12 = left.m12*right.m0 + left.m13*right.m4 + left.m14*right.m8 + left.m15*right.m12;
result.m13 = left.m12*right.m1 + left.m13*right.m5 + left.m14*right.m9 + left.m15*right.m13;
result.m14 = left.m12*right.m2 + left.m13*right.m6 + left.m14*right.m10 + left.m15*right.m14;
result.m15 = left.m12*right.m3 + left.m13*right.m7 + left.m14*right.m11 + left.m15*right.m15;
return result;
}
// Returns translation matrix
RMDEF Matrix MatrixTranslate(float x, float y, float z)
{
Matrix result = { 1.0f, 0.0f, 0.0f, x,
0.0f, 1.0f, 0.0f, y,
0.0f, 0.0f, 1.0f, z,
0.0f, 0.0f, 0.0f, 1.0f };
return result;
}
// Create rotation matrix from axis and angle
// NOTE: Angle should be provided in radians
RMDEF Matrix MatrixRotate(Vector3 axis, float angle)
{
Matrix result = { 0 };
float x = axis.x, y = axis.y, z = axis.z;
float lengthSquared = x*x + y*y + z*z;
if ((lengthSquared != 1.0f) && (lengthSquared != 0.0f))
{
float inverseLength = 1.0f/sqrtf(lengthSquared);
x *= inverseLength;
y *= inverseLength;
z *= inverseLength;
}
float sinres = sinf(angle);
float cosres = cosf(angle);
float t = 1.0f - cosres;
result.m0 = x*x*t + cosres;
result.m1 = y*x*t + z*sinres;
result.m2 = z*x*t - y*sinres;
result.m3 = 0.0f;
result.m4 = x*y*t - z*sinres;
result.m5 = y*y*t + cosres;
result.m6 = z*y*t + x*sinres;
result.m7 = 0.0f;
result.m8 = x*z*t + y*sinres;
result.m9 = y*z*t - x*sinres;
result.m10 = z*z*t + cosres;
result.m11 = 0.0f;
result.m12 = 0.0f;
result.m13 = 0.0f;
result.m14 = 0.0f;
result.m15 = 1.0f;
return result;
}
// Returns x-rotation matrix (angle in radians)
RMDEF Matrix MatrixRotateX(float angle)
{
Matrix result = MatrixIdentity();
float cosres = cosf(angle);
float sinres = sinf(angle);
result.m5 = cosres;
result.m6 = -sinres;
result.m9 = sinres;
result.m10 = cosres;
return result;
}
// Returns y-rotation matrix (angle in radians)
RMDEF Matrix MatrixRotateY(float angle)
{
Matrix result = MatrixIdentity();
float cosres = cosf(angle);
float sinres = sinf(angle);
result.m0 = cosres;
result.m2 = sinres;
result.m8 = -sinres;
result.m10 = cosres;
return result;
}
// Returns z-rotation matrix (angle in radians)
RMDEF Matrix MatrixRotateZ(float angle)
{
Matrix result = MatrixIdentity();
float cosres = cosf(angle);
float sinres = sinf(angle);
result.m0 = cosres;
result.m1 = -sinres;
result.m4 = sinres;
result.m5 = cosres;
return result;
}
// Returns xyz-rotation matrix (angles in radians)
RMDEF Matrix MatrixRotateXYZ(Vector3 ang)
{
Matrix result = MatrixIdentity();
float cosz = cosf(-ang.z);
float sinz = sinf(-ang.z);
float cosy = cosf(-ang.y);
float siny = sinf(-ang.y);
float cosx = cosf(-ang.x);
float sinx = sinf(-ang.x);
result.m0 = cosz * cosy;
result.m4 = (cosz * siny * sinx) - (sinz * cosx);
result.m8 = (cosz * siny * cosx) + (sinz * sinx);
result.m1 = sinz * cosy;
result.m5 = (sinz * siny * sinx) + (cosz * cosx);
result.m9 = (sinz * siny * cosx) - (cosz * sinx);
result.m2 = -siny;
result.m6 = cosy * sinx;
result.m10= cosy * cosx;
return result;
}
// Returns zyx-rotation matrix (angles in radians)
RMDEF Matrix MatrixRotateZYX(Vector3 ang)
{
Matrix result = { 0 };
float cz = cosf(ang.z);
float sz = sinf(ang.z);
float cy = cosf(ang.y);
float sy = sinf(ang.y);
float cx = cosf(ang.x);
float sx = sinf(ang.x);
result.m0 = cz*cy;
result.m1 = cz*sy*sx - cx*sz;
result.m2 = sz*sx + cz*cx*sy;
result.m3 = 0;
result.m4 = cy*sz;
result.m5 = cz*cx + sz*sy*sx;
result.m6 = cx*sz*sy - cz*sx;
result.m7 = 0;
result.m8 = -sy;
result.m9 = cy*sx;
result.m10 = cy*cx;
result.m11 = 0;
result.m12 = 0;
result.m13 = 0;
result.m14 = 0;
result.m15 = 1;
return result;
}
// Returns scaling matrix
RMDEF Matrix MatrixScale(float x, float y, float z)
{
Matrix result = { x, 0.0f, 0.0f, 0.0f,
0.0f, y, 0.0f, 0.0f,
0.0f, 0.0f, z, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f };
return result;
}
// Returns perspective projection matrix
RMDEF Matrix MatrixFrustum(double left, double right, double bottom, double top, double near, double far)
{
Matrix result = { 0 };
float rl = (float)(right - left);
float tb = (float)(top - bottom);
float fn = (float)(far - near);
result.m0 = ((float) near*2.0f)/rl;
result.m1 = 0.0f;
result.m2 = 0.0f;
result.m3 = 0.0f;
result.m4 = 0.0f;
result.m5 = ((float) near*2.0f)/tb;
result.m6 = 0.0f;
result.m7 = 0.0f;
result.m8 = ((float)right + (float)left)/rl;
result.m9 = ((float)top + (float)bottom)/tb;
result.m10 = -((float)far + (float)near)/fn;
result.m11 = -1.0f;
result.m12 = 0.0f;
result.m13 = 0.0f;
result.m14 = -((float)far*(float)near*2.0f)/fn;
result.m15 = 0.0f;
return result;
}
// Returns perspective projection matrix
// NOTE: Angle should be provided in radians
RMDEF Matrix MatrixPerspective(double fovy, double aspect, double near, double far)
{
double top = near*tan(fovy*0.5);
double right = top*aspect;
Matrix result = MatrixFrustum(-right, right, -top, top, near, far);
return result;
}
// Returns orthographic projection matrix
RMDEF Matrix MatrixOrtho(double left, double right, double bottom, double top, double near, double far)
{
Matrix result = { 0 };
float rl = (float)(right - left);
float tb = (float)(top - bottom);
float fn = (float)(far - near);
result.m0 = 2.0f/rl;
result.m1 = 0.0f;
result.m2 = 0.0f;
result.m3 = 0.0f;
result.m4 = 0.0f;
result.m5 = 2.0f/tb;
result.m6 = 0.0f;
result.m7 = 0.0f;
result.m8 = 0.0f;
result.m9 = 0.0f;
result.m10 = -2.0f/fn;
result.m11 = 0.0f;
result.m12 = -((float)left + (float)right)/rl;
result.m13 = -((float)top + (float)bottom)/tb;
result.m14 = -((float)far + (float)near)/fn;
result.m15 = 1.0f;
return result;
}
// Returns camera look-at matrix (view matrix)
RMDEF Matrix MatrixLookAt(Vector3 eye, Vector3 target, Vector3 up)
{
Matrix result = { 0 };
Vector3 z = Vector3Subtract(eye, target);
z = Vector3Normalize(z);
Vector3 x = Vector3CrossProduct(up, z);
x = Vector3Normalize(x);
Vector3 y = Vector3CrossProduct(z, x);
result.m0 = x.x;
result.m1 = y.x;
result.m2 = z.x;
result.m3 = 0.0f;
result.m4 = x.y;
result.m5 = y.y;
result.m6 = z.y;
result.m7 = 0.0f;
result.m8 = x.z;
result.m9 = y.z;
result.m10 = z.z;
result.m11 = 0.0f;
result.m12 = -Vector3DotProduct(x, eye);
result.m13 = -Vector3DotProduct(y, eye);
result.m14 = -Vector3DotProduct(z, eye);
result.m15 = 1.0f;
return result;
}
// Returns float array of matrix data
RMDEF float16 MatrixToFloatV(Matrix mat)
{
float16 buffer = { 0 };
buffer.v[0] = mat.m0;
buffer.v[1] = mat.m1;
buffer.v[2] = mat.m2;
buffer.v[3] = mat.m3;
buffer.v[4] = mat.m4;
buffer.v[5] = mat.m5;
buffer.v[6] = mat.m6;
buffer.v[7] = mat.m7;
buffer.v[8] = mat.m8;
buffer.v[9] = mat.m9;
buffer.v[10] = mat.m10;
buffer.v[11] = mat.m11;
buffer.v[12] = mat.m12;
buffer.v[13] = mat.m13;
buffer.v[14] = mat.m14;
buffer.v[15] = mat.m15;
return buffer;
}
//----------------------------------------------------------------------------------
// Module Functions Definition - Quaternion math
//----------------------------------------------------------------------------------
// Add two quaternions
RMDEF Quaternion QuaternionAdd(Quaternion q1, Quaternion q2)
{
Quaternion result = {q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w};
return result;
}
// Add quaternion and float value
RMDEF Quaternion QuaternionAddValue(Quaternion q, float add)
{
Quaternion result = {q.x + add, q.y + add, q.z + add, q.w + add};
return result;
}
// Subtract two quaternions
RMDEF Quaternion QuaternionSubtract(Quaternion q1, Quaternion q2)
{
Quaternion result = {q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w};
return result;
}
// Subtract quaternion and float value
RMDEF Quaternion QuaternionSubtractValue(Quaternion q, float sub)
{
Quaternion result = {q.x - sub, q.y - sub, q.z - sub, q.w - sub};
return result;
}
// Returns identity quaternion
RMDEF Quaternion QuaternionIdentity(void)
{
Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f };
return result;
}
// Computes the length of a quaternion
RMDEF float QuaternionLength(Quaternion q)
{
float result = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
return result;
}
// Normalize provided quaternion
RMDEF Quaternion QuaternionNormalize(Quaternion q)
{
Quaternion result = { 0 };
float length, ilength;
length = QuaternionLength(q);
if (length == 0.0f) length = 1.0f;
ilength = 1.0f/length;
result.x = q.x*ilength;
result.y = q.y*ilength;
result.z = q.z*ilength;
result.w = q.w*ilength;
return result;
}
// Invert provided quaternion
RMDEF Quaternion QuaternionInvert(Quaternion q)
{
Quaternion result = q;
float length = QuaternionLength(q);
float lengthSq = length*length;
if (lengthSq != 0.0)
{
float i = 1.0f/lengthSq;
result.x *= -i;
result.y *= -i;
result.z *= -i;
result.w *= i;
}
return result;
}
// Calculate two quaternion multiplication
RMDEF Quaternion QuaternionMultiply(Quaternion q1, Quaternion q2)
{
Quaternion result = { 0 };
float qax = q1.x, qay = q1.y, qaz = q1.z, qaw = q1.w;
float qbx = q2.x, qby = q2.y, qbz = q2.z, qbw = q2.w;
result.x = qax*qbw + qaw*qbx + qay*qbz - qaz*qby;
result.y = qay*qbw + qaw*qby + qaz*qbx - qax*qbz;
result.z = qaz*qbw + qaw*qbz + qax*qby - qay*qbx;
result.w = qaw*qbw - qax*qbx - qay*qby - qaz*qbz;
return result;
}
// Scale quaternion by float value
RMDEF Quaternion QuaternionScale(Quaternion q, float mul)
{
Quaternion result = { 0 };
float qax = q.x, qay = q.y, qaz = q.z, qaw = q.w;
result.x = qax * mul + qaw * mul + qay * mul - qaz * mul;
result.y = qay * mul + qaw * mul + qaz * mul - qax * mul;
result.z = qaz * mul + qaw * mul + qax * mul - qay * mul;
result.w = qaw * mul - qax * mul - qay * mul - qaz * mul;
return result;
}
// Divide two quaternions
RMDEF Quaternion QuaternionDivide(Quaternion q1, Quaternion q2)
{
Quaternion result = {q1.x / q2.x, q1.y / q2.y, q1.z / q2.z, q1.w / q2.w};
return result;
}
// Calculate linear interpolation between two quaternions
RMDEF Quaternion QuaternionLerp(Quaternion q1, Quaternion q2, float amount)
{
Quaternion result = { 0 };
result.x = q1.x + amount*(q2.x - q1.x);
result.y = q1.y + amount*(q2.y - q1.y);
result.z = q1.z + amount*(q2.z - q1.z);
result.w = q1.w + amount*(q2.w - q1.w);
return result;
}
// Calculate slerp-optimized interpolation between two quaternions
RMDEF Quaternion QuaternionNlerp(Quaternion q1, Quaternion q2, float amount)
{
Quaternion result = QuaternionLerp(q1, q2, amount);
result = QuaternionNormalize(result);
return result;
}
// Calculates spherical linear interpolation between two quaternions
RMDEF Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float amount)
{
Quaternion result = { 0 };
float cosHalfTheta = q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
if (cosHalfTheta < 0)
{
q2.x = -q2.x; q2.y = -q2.y; q2.z = -q2.z; q2.w = -q2.w;
cosHalfTheta = -cosHalfTheta;
}
if (fabs(cosHalfTheta) >= 1.0f) result = q1;
else if (cosHalfTheta > 0.95f) result = QuaternionNlerp(q1, q2, amount);
else
{
float halfTheta = acosf(cosHalfTheta);
float sinHalfTheta = sqrtf(1.0f - cosHalfTheta*cosHalfTheta);
if (fabs(sinHalfTheta) < 0.001f)
{
result.x = (q1.x*0.5f + q2.x*0.5f);
result.y = (q1.y*0.5f + q2.y*0.5f);
result.z = (q1.z*0.5f + q2.z*0.5f);
result.w = (q1.w*0.5f + q2.w*0.5f);
}
else
{
float ratioA = sinf((1 - amount)*halfTheta)/sinHalfTheta;
float ratioB = sinf(amount*halfTheta)/sinHalfTheta;
result.x = (q1.x*ratioA + q2.x*ratioB);
result.y = (q1.y*ratioA + q2.y*ratioB);
result.z = (q1.z*ratioA + q2.z*ratioB);
result.w = (q1.w*ratioA + q2.w*ratioB);
}
}
return result;
}
// Calculate quaternion based on the rotation from one vector to another
RMDEF Quaternion QuaternionFromVector3ToVector3(Vector3 from, Vector3 to)
{
Quaternion result = { 0 };
float cos2Theta = Vector3DotProduct(from, to);
Vector3 cross = Vector3CrossProduct(from, to);
result.x = cross.x;
result.y = cross.y;
result.z = cross.z;
result.w = 1.0f + cos2Theta; // NOTE: Added QuaternioIdentity()
// Normalize to essentially nlerp the original and identity to 0.5
result = QuaternionNormalize(result);
// Above lines are equivalent to:
//Quaternion result = QuaternionNlerp(q, QuaternionIdentity(), 0.5f);
return result;
}
// Returns a quaternion for a given rotation matrix
RMDEF Quaternion QuaternionFromMatrix(Matrix mat)
{
Quaternion result = { 0 };
if ((mat.m0 > mat.m5) && (mat.m0 > mat.m10))
{
float s = sqrtf(1.0f + mat.m0 - mat.m5 - mat.m10)*2;
result.x = 0.25f*s;
result.y = (mat.m4 + mat.m1)/s;
result.z = (mat.m2 + mat.m8)/s;
result.w = (mat.m9 - mat.m6)/s;
}
else if (mat.m5 > mat.m10)
{
float s = sqrtf(1.0f + mat.m5 - mat.m0 - mat.m10)*2;
result.x = (mat.m4 + mat.m1)/s;
result.y = 0.25f*s;
result.z = (mat.m9 + mat.m6)/s;
result.w = (mat.m2 - mat.m8)/s;
}
else
{
float s = sqrtf(1.0f + mat.m10 - mat.m0 - mat.m5)*2;
result.x = (mat.m2 + mat.m8)/s;
result.y = (mat.m9 + mat.m6)/s;
result.z = 0.25f*s;
result.w = (mat.m4 - mat.m1)/s;
}
return result;
}
// Returns a matrix for a given quaternion
RMDEF Matrix QuaternionToMatrix(Quaternion q)
{
Matrix result = MatrixIdentity();
float a2 = 2*(q.x*q.x), b2=2*(q.y*q.y), c2=2*(q.z*q.z); //, d2=2*(q.w*q.w);
float ab = 2*(q.x*q.y), ac=2*(q.x*q.z), bc=2*(q.y*q.z);
float ad = 2*(q.x*q.w), bd=2*(q.y*q.w), cd=2*(q.z*q.w);
result.m0 = 1 - b2 - c2;
result.m1 = ab - cd;
result.m2 = ac + bd;
result.m4 = ab + cd;
result.m5 = 1 - a2 - c2;
result.m6 = bc - ad;
result.m8 = ac - bd;
result.m9 = bc + ad;
result.m10 = 1 - a2 - b2;
return result;
}
// Returns rotation quaternion for an angle and axis
// NOTE: angle must be provided in radians
RMDEF Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle)
{
Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f };
if (Vector3Length(axis) != 0.0f)
angle *= 0.5f;
axis = Vector3Normalize(axis);
float sinres = sinf(angle);
float cosres = cosf(angle);
result.x = axis.x*sinres;
result.y = axis.y*sinres;
result.z = axis.z*sinres;
result.w = cosres;
result = QuaternionNormalize(result);
return result;
}
// Returns the rotation angle and axis for a given quaternion
RMDEF void QuaternionToAxisAngle(Quaternion q, Vector3 *outAxis, float *outAngle)
{
if (fabs(q.w) > 1.0f) q = QuaternionNormalize(q);
Vector3 resAxis = { 0.0f, 0.0f, 0.0f };
float resAngle = 2.0f*acosf(q.w);
float den = sqrtf(1.0f - q.w*q.w);
if (den > 0.0001f)
{
resAxis.x = q.x/den;
resAxis.y = q.y/den;
resAxis.z = q.z/den;
}
else
{
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
resAxis.x = 1.0f;
}
*outAxis = resAxis;
*outAngle = resAngle;
}
// Returns the quaternion equivalent to Euler angles
// NOTE: Rotation order is ZYX
RMDEF Quaternion QuaternionFromEuler(float pitch, float yaw, float roll)
{
Quaternion q = { 0 };
float x0 = cosf(pitch*0.5f);
float x1 = sinf(pitch*0.5f);
float y0 = cosf(yaw*0.5f);
float y1 = sinf(yaw*0.5f);
float z0 = cosf(roll*0.5f);
float z1 = sinf(roll*0.5f);
q.x = x1*y0*z0 - x0*y1*z1;
q.y = x0*y1*z0 + x1*y0*z1;
q.z = x0*y0*z1 - x1*y1*z0;
q.w = x0*y0*z0 + x1*y1*z1;
return q;
}
// Return the Euler angles equivalent to quaternion (roll, pitch, yaw)
// NOTE: Angles are returned in a Vector3 struct in degrees
RMDEF Vector3 QuaternionToEuler(Quaternion q)
{
Vector3 result = { 0 };
// roll (x-axis rotation)
float x0 = 2.0f*(q.w*q.x + q.y*q.z);
float x1 = 1.0f - 2.0f*(q.x*q.x + q.y*q.y);
result.x = atan2f(x0, x1)*RAD2DEG;
// pitch (y-axis rotation)
float y0 = 2.0f*(q.w*q.y - q.z*q.x);
y0 = y0 > 1.0f ? 1.0f : y0;
y0 = y0 < -1.0f ? -1.0f : y0;
result.y = asinf(y0)*RAD2DEG;
// yaw (z-axis rotation)
float z0 = 2.0f*(q.w*q.z + q.x*q.y);
float z1 = 1.0f - 2.0f*(q.y*q.y + q.z*q.z);
result.z = atan2f(z0, z1)*RAD2DEG;
return result;
}
// Transform a quaternion given a transformation matrix
RMDEF Quaternion QuaternionTransform(Quaternion q, Matrix mat)
{
Quaternion result = { 0 };
result.x = mat.m0*q.x + mat.m4*q.y + mat.m8*q.z + mat.m12*q.w;
result.y = mat.m1*q.x + mat.m5*q.y + mat.m9*q.z + mat.m13*q.w;
result.z = mat.m2*q.x + mat.m6*q.y + mat.m10*q.z + mat.m14*q.w;
result.w = mat.m3*q.x + mat.m7*q.y + mat.m11*q.z + mat.m15*q.w;
return result;
}
// Projects a Vector3 from screen space into object space
RMDEF Vector3 Vector3Unproject(Vector3 source, Matrix projection, Matrix view)
{
Vector3 result = { 0.0f, 0.0f, 0.0f };
// Calculate unproject matrix (multiply view patrix by projection matrix) and invert it
Matrix matViewProj = MatrixMultiply(view, projection);
matViewProj = MatrixInvert(matViewProj);
// Create quaternion from source point
Quaternion quat = { source.x, source.y, source.z, 1.0f };
// Multiply quat point by unproject matrix
quat = QuaternionTransform(quat, matViewProj);
// Normalized world points in vectors
result.x = quat.x/quat.w;
result.y = quat.y/quat.w;
result.z = quat.z/quat.w;
return result;
}
#endif // RAYMATH_H
dpd/include/rlgl.h 0 → 100644
View file @ b0598b21
This source diff could not be displayed because it is too large. You can view the blob instead.
dpd/libraylib.a 0 → 100644
View file @ b0598b21
File added
draf.md 0 → 100644
View file @ b0598b21
```mermaid
graph TD;
Antho-->Bobby;
Antho-->Clariss;
Bobby-->Dede;
Clariss-->Dede;
```
## ToDo :
- Inclure IGraph
project-outline.md
View file @ b0598b21
......@@ -31,8 +31,8 @@ This functionality cover a framework for the team of developers.
NetWorld represent the center piece of the game engine. It is the programme component that glue all the others.
It is an environment for the game entities, viewed as a planar graph modelling all the possible movements.
- A NetWorld is composed of nodes at specific position (x,y).
- Nodes are connected to other with edges.
- A NetWorld is composed of nodes at specific position (x,y). **DONE**
- Nodes are connected to other with edges. **DONE**
- Nodes can contain entities.
- It is possible to generate NetWorld randomly (example: random nodes and Gabriel graph).
- An algorithm provides paths in the NetWorld between two positions (A*).
......@@ -105,4 +105,7 @@ The goal here is to distribute the game process.
## Fct.7 - Tanks
Nodes are defined with a collection of tanks where resources can be placed. (water for instance)
Nodes are defined with a collection of digital tanks where resources can be placed. (water for instance)
- Digit represent a discrete value in a fixed capacity tank.
- Node contain a collection of tanks as Digit.
src/controlpanel.c 0 → 100644
View file @ b0598b21
#include "controlpanel.h"
#include "raylib.h"
#include "raymath.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
// Tools
void printVector2( Vector2 v )
{
printf( "[%3f, %3f]", v.x, v.y );
}
// Constructor / Destructor
Panel * Panel_new()
{
Panel * p = malloc( sizeof(Panel) );
return p;
}
void Panel_delete(Panel * self)
{
free(self);
}
// Initialization
void Panel_initialize(Panel * self, NetWorld * world)
{
self->world= world;
self->camera.x= 0.f;
self->camera.y= 0.f;
self->screenCenter.x= 400.f;
self->screenCenter.y= 300.f;
self->scale= 10.f; //pixel per meters
}
// To String
void Panel_print(Panel * self)
{
printf( "Panel camera(x-%f, y-%f, scale-%f)\n",
self->camera.x,
self->camera.y,
self->scale);
}
// Rendering
void Panel_draw(Panel * self)
{
BeginDrawing();
ClearBackground(RAYWHITE);
// Draw the edges for each nodes:
for(int i= 0 ; i < self->world->size ; ++i )
{
Node * n= &(self->world->nodes[i]);
for(int j= 0 ; j < n->card ; ++j )
Panel_drawEdge( self, &(n->edges[j]) );
}
// Draw the nodes
for(int i= 0 ; i < self->world->size ; ++i )
{
Panel_drawNode( self, &(self->world->nodes[i]) );
}
Panel_drawBasis(self);
EndDrawing();
}
void Panel_drawBasis(Panel * self)
{
Vector2 screen00= {0.f, 0.f};
screen00= Panel_pixelFromPosition(self, &( screen00 ) );
Vector2 screen10= {1.f, 0.f};
screen10= Panel_pixelFromPosition(self, &( screen10 ) );
Vector2 screen01= {0.f, 1.f};
screen01= Panel_pixelFromPosition(self, &( screen01 ) );
DrawCircleV( screen00, 4, BLUE );
DrawLineV( screen00, screen10, RED );
DrawLineV( screen00, screen01, BLUE );
}
void Panel_drawNode(Panel * self, Node * n)
{
Vector2 screenPosition= Panel_pixelFromPosition(self, &(n->position) );
DrawCircleV(screenPosition, 24, n->color);
DrawCircleV(screenPosition, 20, RAYWHITE);
DrawText(n->name, (int)(screenPosition.x), (int)(screenPosition.y), 20, n->color);
}
void Panel_drawEdge(Panel * self, Edge * e)
{
Vector2 source= Panel_pixelFromPosition(self, &(e->_source->position) );
Vector2 target= Panel_pixelFromPosition(self, &(e->_target->position) );
Vector2 ortho= (Vector2){ target.x-source.x, target.y-source.y }; // Vector from Source to Target
float ratio= 10.f / Vector2Length( ortho );
ortho= (Vector2){ -ortho.y*ratio, ortho.x*ratio };// 10 ortho-normal vector from v
Vector2 source1= (Vector2){source.x-ortho.x, source.y-ortho.y};
Vector2 source2= (Vector2){source.x+ortho.x, source.y+ortho.y};
DrawTriangle(source1, source2, target, e->color);
}
Vector2 Panel_pixelFromPosition(Panel * self, Vector2 * p)
{
Vector2 pixel= {
self->screenCenter.x + (p->x - self->camera.x ) * self->scale,
self->screenCenter.y - (p->y - self->camera.y ) * self->scale
};
return pixel;
}
Vector2 Panel_positionFromPixel(Panel * self, Vector2 * p)
{
Vector2 position= { p->x, p->y };
return position;
}
void Panel_control(Panel * self)
{
Panel_controlCamera(self);
}
void Panel_controlCamera(Panel * self)
{
// KEYBOARD Control:
float step= 3.0f / self->scale;
if (IsKeyDown(KEY_RIGHT)) self->camera.x += step;
if (IsKeyDown(KEY_LEFT)) self->camera.x -= step;
if (IsKeyDown(KEY_UP)) self->camera.y += step;
if (IsKeyDown(KEY_DOWN)) self->camera.y -= step;
self->scale += (GetMouseWheelMove()*1.f);
self->scale = fmaxf( self->scale, 0.001f );
}
\ No newline at end of file
src/controlpanel.h 0 → 100644
View file @ b0598b21
#ifndef CONTROLPANEL_H
#define CONTROLPANEL_H
#include "networld.h"
// Tools
void printVector2( Vector2 v );
//-----------------------------------//
//-- Cotrol Panel --//
//-----------------------------------//
struct Str_Panel {
Vector2 camera, screenCenter;
float scale; // meter per pixel
NetWorld * world;
};
typedef struct Str_Panel Panel;
// Constructor / Destructor
Panel * Panel_new();
void Panel_delete(Panel * self);
// Initialization
void Panel_initialize(Panel * self, NetWorld * world);
// To String
void Panel_print(Panel * self);
// Rendering
void Panel_draw(Panel * self);
void Panel_drawBasis(Panel * self);
void Panel_drawNode(Panel * self, Node * n);
void Panel_drawEdge(Panel * self, Edge * e);
Vector2 Panel_pixelFromPosition(Panel * self, Vector2 * p);
Vector2 Panel_positionFromPixel(Panel * self, Vector2 * p);
// Control
void Panel_control(Panel * self);
void Panel_controlCamera(Panel * self);
#endif //CONTROLPANEL_H
\ No newline at end of file
src/entity.c 0 → 100644
View file @ b0598b21
#include "entity.h"
src/entity.h 0 → 100644
View file @ b0598b21
#ifndef ENTITY_H
#define ENTITY_H
#include "raylib.h"
//-----------------------------------//
//-- Entity Descriptor --//
//-----------------------------------//
struct Str_EntityDsc {
Color color;
};
typedef struct Str_EntityDsc EntityDsc;
struct Str_Entity {
//! Owner of the Entity (classically the player Id)
int owner;
EntityDsc descriptor;
};
typedef struct Str_Entity Entity;
#endif // ENTITY_H
\ No newline at end of file
src/main-viewer.c
View file @ b0598b21
......@@ -5,19 +5,21 @@
*
********************************************************************************************/
#include "networld.h"
#include "raylib.h"
#include <stdlib.h>
#include <stdio.h>
#include "networld.h"
#include "controlpanel.h"
// Program attributes
//-------------------
const int screenWidth = 800;
const int screenHeight = 450;
const int screenHeight = 600;
const int targetFPS = 60;
void game_update(NetWorld * world);
void game_draw(NetWorld * world);
// Game attributes
//-----------------
......@@ -29,9 +31,16 @@ int main(int nbArg, char ** arg)
//--------------------
game_end= false;
NetWorld * world= NetWorld_new(3);
NetWorld_initNodePosition( world, 0, 10.4, 12.8 );
NetWorld_initNodePosition( world, 1, 110.4, 52.8 );
NetWorld_initNodePosition( world, 2, 384.5, 422.2 );
Node_set( &(world->nodes[0]), (Vector2){1.4f, 1.28f}, RED );
Node_set( &(world->nodes[1]), (Vector2){21.0f, 22.8f}, MAGENTA );
Node_set( &(world->nodes[2]), (Vector2){18.4f, -12.2f}, GRAY );
NetWorld_connect(world, 0, 1);
NetWorld_connect(world, 1, 2);
NetWorld_biconnect(world, 2, 0);
Panel * panel= Panel_new();
Panel_initialize(panel, world);
// Raylib Initialization
//----------------------
......@@ -43,12 +52,17 @@ int main(int nbArg, char ** arg)
puts("world variable:");
NetWorld_print(world);
puts("world expected:\n[10.4, 12.8]\n[110.4, 52.8]\n[384.5, 422.2]");
Panel_print(panel);
Vector2 position= {1.f, 1.f};
position= Panel_pixelFromPosition( panel, &position );
printf("[%.2f,%.2f]\n", position.x, position.y);
// Main game loop
while (!game_end && !WindowShouldClose()) // Detect window close button or ESC key
{
Panel_control(panel);
game_update(world);
game_draw(world);
Panel_draw(panel);
}
// proper closing
......@@ -63,15 +77,3 @@ void game_update(NetWorld * world)
{
}
void game_draw(NetWorld * world)
{
BeginDrawing();
ClearBackground(RAYWHITE);
for(int i= 0 ; i < world->size ; ++i )
{
Vector2 nodePosition= { world->nodes[i].x, world->nodes[i].y };
DrawCircleV(nodePosition, 24, MAROON);
}
EndDrawing();
}
src/networld.c
View file @ b0598b21
#include "networld.h"
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <string.h>
//-----------------------------------//
//-- Node --//
//-----------------------------------//
// Constructor / Destructor
void Node_construct(Node * self)
{
Node_set( self, (Vector2){0.f, 0.f}, GRAY );
self->card= 0;
self->edges= Edge_newArray( 0 );
self->name= malloc( sizeof(char)*32 );
strcpy( self->name, "Node" );
}
Node * Node_new()
{
Node * p= malloc( sizeof(Node) );
Node_construct(p);
return p;
}
Node * Node_newArray(int size)
{
size= fmaxf(1, size);
Node * p= malloc( sizeof(Node)*size );
for(int i=0 ; i < size ; ++i )
{
Node_construct( &(p[i]) );
}
return p;
}
void Node_delete( Node * node )
{
Edge_deleteArray( node->card, node->edges );
free( node );
}
/**
* @brief Destructor of an array of nodes.
*/
void Node_deleteArray( int n, Node * array )
{
for(int i= 0 ; i < n ; ++i )
Edge_deleteArray( array[i].card, array[i].edges );
free( array );
}
/**
* @brief initialize a node with a number of edges
*/
void Node_initialize( Node * self, int card )
{
for(int i= 0 ; i < self->card ; ++i )
Edge_deleteArray( self->card, self->edges );
self->edges= Edge_newArray( card );
self->card= card;
}
/**
* @brief Resize
*/
void Node_resize( Node * self, int card )
{
Edge * newEdges= Edge_newArray(card);
int minCard= fminf(self->card, card);
for( int i = 0 ; i < minCard ; ++i )
{
Edge_copy( &newEdges[i], &(self->edges[i]) );
}
Edge_deleteArray( self->card, self->edges );
self->edges= newEdges;
self->card= card;
}
void Node_set( Node * self, Vector2 position, Color color )
{
self->position= position;
self->color= color;
}
int Node_connect( Node * self, Node * target )
{
int i= self->card;
Node_resize( self, i+1 );
self->edges[i]._source= self;
self->edges[i]._target= target;
self->edges[i].color= self->color;
self->edges[i]._twin= NULL;
return i;
}
int Node_biconnect( Node * node1, Node * node2 )
{
int i1= Node_connect(node1, node2);
int i2= Node_connect(node2, node1);
node1->edges[i1]._twin= &(node2->edges[i2]);
node2->edges[i2]._twin= &(node1->edges[i1]);
return i1;
}
//-----------------------------------//
//-- Edge --//
//-----------------------------------//
// Constructor / Destructor
void Edge_construct( Edge * self, Node * source, Node * target )
{
self->_source= source;
self->_target= target;
self->_twin= NULL;
self->color= GRAY;
}
Edge * Edge_new( Node * source, Node * target )
{
Edge * p = malloc( sizeof(Edge) );
Edge_construct( p, source, target );
return p;
}
/**
* @brief Destructor.
*/
void Edge_delete( Edge * self )
{
free( self );
}
/**
* @brief Allocate 'size' Edges (with NULL target)
* @return The pointer to the new array of 'size' Edges.
*/
Edge * Edge_newArray(int size)
{
size= fmaxf(1, size);
Edge * p = malloc( sizeof(Edge)*size );
for( int i = 0 ; i < size ; ++i )
{
Edge_construct( &(p[i]), NULL, NULL );
}
return p;
}
/**
* @brief Destructor.
*/
void Edge_deleteArray( int n, Edge * array )
{
free(array);
}
/**
* @brief Copy an edge from another
*/
void Edge_copy( Edge * copy, Edge * model )
{
copy->_source= model->_source;
copy->_target= model->_target;
copy->_twin= model->_twin;
}
//-----------------------------------//
//-- NetWorld --//
//-----------------------------------//
// Constructor / Destructor
NetWorld * NetWorld_new(int size)
{
NetWorld * p = malloc( sizeof(NetWorld) );
p->size= size;
p->nodes= malloc( p->size * sizeof(Node) );
p->nodes= Node_newArray( p->size );
return p;
}
void NetWorld_delete(NetWorld * self)
{
free( self->nodes );
Node_deleteArray( self->size, self->nodes );
free( self );
}
// Initialization
void NetWorld_initNodePosition(
NetWorld * self, int iNode,
double x, double y)
{
self->nodes[iNode].x= x;
self->nodes[iNode].y= y;
}
// To String
void NetWorld_print(NetWorld * self)
{
for(int i= 0 ; i < self->size ; ++i )
printf("[%lf, %lf]\n", self->nodes[i].x, self->nodes[i].y);
printf("[%.2f, %.2f]%d\n",
self->nodes[i].position.x,
self->nodes[i].position.y,
self->nodes[i].card);
}
// Managment :
int NetWorld_connect( NetWorld * self, int source, int target )
{
return Node_connect( &(self->nodes[source]), &(self->nodes[target]) );
}
int NetWorld_biconnect( NetWorld * self, int source, int target )
{
return Node_biconnect( &(self->nodes[source]), &(self->nodes[target]) );
}
src/networld.h
View file @ b0598b21
#ifndef NETWORLD_H
#define NETWORLD_H
#include "raylib.h"
//-----------------------------------//
//-- Node --//
//-----------------------------------//
struct Str_Edge;
struct Str_Node {
double x, y;
//! position (x, y) of the node
Vector2 position;
//! color (r, g, b, a) of the node
Color color;
struct Str_Edge * edges;
//! cardinality of the node (i.e. number of edges)
int card;
// Content:
//! name of the node
char* name;
};
/**
* @brief NetWorld Node.
*/
typedef struct Str_Node Node;
// Constructor / Destructor
/**
* @brief Construc all the element of an empty Node.
* @param self an empty Node ot yet constructed.
* @return The pointer to the new NetWorld.
*/
void Node_construct(Node * self);
/**
* @brief Allocate the memory to store a Node
* @return The pointer to the new NetWorld.
*/
Node * Node_new();
/**
* @brief Allocate 'size' Nodes.
* @return The pointer to the new array of 'size' Nodes.
*/
Node * Node_newArray(int size);
/**
* @brief Destructor.
*/
void Node_delete(
//! the NetWorld to delete
Node * self
);
/**
* @brief Destructor of an array of nodes.
*/
void Node_deleteArray(
//! the number of nodes to delete
int n,
//! the Nodes
Node * array
);
/**
* @brief initialize a node with a number of edges
*/
void Node_initialize(
//! the node to initialize
Node * self,
//! the number of outgoing edges
int card
);
/**
* @brief Resize
*/
void Node_resize(
//! the node to resize
Node * self,
//! the new number of outgoing edges
int card
);
// Managment :
/**
* @brief set
*/
void Node_set(
//! the node to set
Node * self,
//! position
Vector2 position,
//! color
Color color
);
/**
* @brief connect tow node together
* @return the index of the edge in the node edges' array
*/
int Node_connect(
//! the source Node to modify
Node * self,
//! the target node
Node * target
);
/**
* @brief connect tow node together (bidirectional)
* @return the index of the edge in the node edges' array
*/
int Node_biconnect(
//! the source Node to modify
Node * node1,
//! the target node
Node * node2
);
//-----------------------------------//
//-- Edge --//
//-----------------------------------//
struct Str_Edge {
//! pointer to the source node.
Node * _source;
//! pointer to the connected node.
Node * _target;
//! pointer to the twin edge in case of bidirectional edge.
struct Str_Edge * _twin;
//! color (r, g, b, a) of the node
Color color;
};
/**
* @brief NetWorld Edge.
*/
typedef struct Str_Edge Edge;
// Constructor / Destructor
/**
* @brief Construc all the element of an empty Edge.
* @param self an empty Edge ot yet constructed.
* @param source the source Node the edge start from (default NULL)
* @param target the target Node to connect (default NULL)
* @return The pointer to the new NetWorld.
*/
void Edge_construct( Edge * self, Node * source, Node * target );
/**
* @brief Allocate the memory to store a NetWorld.
* @return The pointer to the new NetWorld.
*/
Edge * Edge_new(
//! the source Node to connect
Node * source,
//! the target Node to connect (NULL if unknown)
Node * target
);
/**
* @brief Destructor.
*/
void Edge_delete(
//! the NetWorld to delete
Edge * self
);
/**
* @brief Allocate 'size' Edges (with NULL target)
* @return The pointer to the new array of 'size' Edges.
*/
Edge * Edge_newArray(int size);
/**
* @brief Destructor.
*/
void Edge_deleteArray(
//! the number of edges
int n,
//! the NetWorld to delete
Edge * array
);
// Copying
/**
* @brief Copy an edge from another
*/
void Edge_copy(
//! the edge to modify
Edge * copy,
//! the edge to copy
Edge * model
);
//-----------------------------------//
//-- NetWorld --//
//-----------------------------------//
struct Str_NetWorld {
//! number of nodes composing the NetWorld
int size;
//! Array of 'size' nodes
Node * nodes;
};
typedef struct Str_NetWorld NetWorld;
// Constructor / Destructor
NetWorld * NetWorld_new(int aSize);
void NetWorld_delete(NetWorld * self);
/**
* @brief Allocate the memory to store a NetWorld.
* @return The pointer to the new NetWorld.
*/
NetWorld * NetWorld_new(
//! the number of Nodes composing the new NetWorld
int aSize
);
// Initialization
void NetWorld_initNodePosition(NetWorld * self, int iNode, double x, double y); // position must be an float[size][2] array...
/**
* @brief Destructor.
*/
void NetWorld_delete(
//! the NetWorld to delete
NetWorld * self
);
// To String
void NetWorld_print(NetWorld * self);
/**
* @brief print
*/
void NetWorld_print(
//! the NetWorld to modify
NetWorld * self
);
// Managment :
/**
* @brief connect tow node together
* @return the index of the edge in the node edges' array
*/
int NetWorld_connect(
//! the NetWorld to modify
NetWorld * self,
//! index of the source node
int source,
//! index of the target node
int target
);
/**
* @brief connect tow node together (bidirectional)
* @return the index of the edge in the node edges' array
*/
int NetWorld_biconnect(
//! the NetWorld to modify
NetWorld * self,
//! index of the source node
int source,
//! index of the target node
int target
);
#endif //NETWORLD_H
\ No newline at end of file
src/raymath.h 0 → 100644
View file @ b0598b21
/**********************************************************************************************
*
* raymath v1.2 - Math functions to work with Vector3, Matrix and Quaternions
*
* CONFIGURATION:
*
* #define RAYMATH_IMPLEMENTATION
* Generates the implementation of the library into the included file.
* If not defined, the library is in header only mode and can be included in other headers
* or source files without problems. But only ONE file should hold the implementation.
*
* #define RAYMATH_HEADER_ONLY
* Define static inline functions code, so #include header suffices for use.
* This may use up lots of memory.
*
* #define RAYMATH_STANDALONE
* Avoid raylib.h header inclusion in this file.
* Vector3 and Matrix data types are defined internally in raymath module.
*
*
* LICENSE: zlib/libpng
*
* Copyright (c) 2015-2020 Ramon Santamaria (@raysan5)
*
* This software is provided "as-is", without any express or implied warranty. In no event
* will the authors be held liable for any damages arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose, including commercial
* applications, and to alter it and redistribute it freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not claim that you
* wrote the original software. If you use this software in a product, an acknowledgment
* in the product documentation would be appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be misrepresented
* as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*
**********************************************************************************************/
#ifndef RAYMATH_H
#define RAYMATH_H
//#define RAYMATH_STANDALONE // NOTE: To use raymath as standalone lib, just uncomment this line
//#define RAYMATH_HEADER_ONLY // NOTE: To compile functions as static inline, uncomment this line
#ifndef RAYMATH_STANDALONE
#include "raylib.h" // Required for structs: Vector3, Matrix
#endif
#if defined(RAYMATH_IMPLEMENTATION) && defined(RAYMATH_HEADER_ONLY)
#error "Specifying both RAYMATH_IMPLEMENTATION and RAYMATH_HEADER_ONLY is contradictory"
#endif
#if defined(RAYMATH_IMPLEMENTATION)
#if defined(_WIN32) && defined(BUILD_LIBTYPE_SHARED)
#define RMDEF __declspec(dllexport) extern inline // We are building raylib as a Win32 shared library (.dll).
#elif defined(_WIN32) && defined(USE_LIBTYPE_SHARED)
#define RMDEF __declspec(dllimport) // We are using raylib as a Win32 shared library (.dll)
#else
#define RMDEF extern inline // Provide external definition
#endif
#elif defined(RAYMATH_HEADER_ONLY)
#define RMDEF static inline // Functions may be inlined, no external out-of-line definition
#else
#if defined(__TINYC__)
#define RMDEF static inline // plain inline not supported by tinycc (See issue #435)
#else
#define RMDEF inline // Functions may be inlined or external definition used
#endif
#endif
//----------------------------------------------------------------------------------
// Defines and Macros
//----------------------------------------------------------------------------------
#ifndef PI
#define PI 3.14159265358979323846
#endif
#ifndef DEG2RAD
#define DEG2RAD (PI/180.0f)
#endif
#ifndef RAD2DEG
#define RAD2DEG (180.0f/PI)
#endif
// Return float vector for Matrix
#ifndef MatrixToFloat
#define MatrixToFloat(mat) (MatrixToFloatV(mat).v)
#endif
// Return float vector for Vector3
#ifndef Vector3ToFloat
#define Vector3ToFloat(vec) (Vector3ToFloatV(vec).v)
#endif
//----------------------------------------------------------------------------------
// Types and Structures Definition
//----------------------------------------------------------------------------------
#if defined(RAYMATH_STANDALONE)
// Vector2 type
typedef struct Vector2 {
float x;
float y;
} Vector2;
// Vector3 type
typedef struct Vector3 {
float x;
float y;
float z;
} Vector3;
// Quaternion type
typedef struct Quaternion {
float x;
float y;
float z;
float w;
} Quaternion;
// Matrix type (OpenGL style 4x4 - right handed, column major)
typedef struct Matrix {
float m0, m4, m8, m12;
float m1, m5, m9, m13;
float m2, m6, m10, m14;
float m3, m7, m11, m15;
} Matrix;
#endif
// NOTE: Helper types to be used instead of array return types for *ToFloat functions
typedef struct float3 { float v[3]; } float3;
typedef struct float16 { float v[16]; } float16;
#include <math.h> // Required for: sinf(), cosf(), sqrtf(), tan(), fabs()
//----------------------------------------------------------------------------------
// Module Functions Definition - Utils math
//----------------------------------------------------------------------------------
// Clamp float value
RMDEF float Clamp(float value, float min, float max)
{
const float res = value < min ? min : value;
return res > max ? max : res;
}
// Calculate linear interpolation between two floats
RMDEF float Lerp(float start, float end, float amount)
{
return start + amount*(end - start);
}
//----------------------------------------------------------------------------------
// Module Functions Definition - Vector2 math
//----------------------------------------------------------------------------------
// Vector with components value 0.0f
RMDEF Vector2 Vector2Zero(void)
{
Vector2 result = { 0.0f, 0.0f };
return result;
}
// Vector with components value 1.0f
RMDEF Vector2 Vector2One(void)
{
Vector2 result = { 1.0f, 1.0f };
return result;
}
// Add two vectors (v1 + v2)
RMDEF Vector2 Vector2Add(Vector2 v1, Vector2 v2)
{
Vector2 result = { v1.x + v2.x, v1.y + v2.y };
return result;
}
// Subtract two vectors (v1 - v2)
RMDEF Vector2 Vector2Subtract(Vector2 v1, Vector2 v2)
{
Vector2 result = { v1.x - v2.x, v1.y - v2.y };
return result;
}
// Calculate vector length
RMDEF float Vector2Length(Vector2 v)
{
float result = sqrtf((v.x*v.x) + (v.y*v.y));
return result;
}
// Calculate two vectors dot product
RMDEF float Vector2DotProduct(Vector2 v1, Vector2 v2)
{
float result = (v1.x*v2.x + v1.y*v2.y);
return result;
}
// Calculate distance between two vectors
RMDEF float Vector2Distance(Vector2 v1, Vector2 v2)
{
float result = sqrtf((v1.x - v2.x)*(v1.x - v2.x) + (v1.y - v2.y)*(v1.y - v2.y));
return result;
}
// Calculate angle from two vectors in X-axis
RMDEF float Vector2Angle(Vector2 v1, Vector2 v2)
{
float result = atan2f(v2.y - v1.y, v2.x - v1.x)*(180.0f/PI);
if (result < 0) result += 360.0f;
return result;
}
// Scale vector (multiply by value)
RMDEF Vector2 Vector2Scale(Vector2 v, float scale)
{
Vector2 result = { v.x*scale, v.y*scale };
return result;
}
// Multiply vector by vector
RMDEF Vector2 Vector2MultiplyV(Vector2 v1, Vector2 v2)
{
Vector2 result = { v1.x*v2.x, v1.y*v2.y };
return result;
}
// Negate vector
RMDEF Vector2 Vector2Negate(Vector2 v)
{
Vector2 result = { -v.x, -v.y };
return result;
}
// Divide vector by a float value
RMDEF Vector2 Vector2Divide(Vector2 v, float div)
{
Vector2 result = { v.x/div, v.y/div };
return result;
}
// Divide vector by vector
RMDEF Vector2 Vector2DivideV(Vector2 v1, Vector2 v2)
{
Vector2 result = { v1.x/v2.x, v1.y/v2.y };
return result;
}
// Normalize provided vector
RMDEF Vector2 Vector2Normalize(Vector2 v)
{
Vector2 result = Vector2Divide(v, Vector2Length(v));
return result;
}
// Calculate linear interpolation between two vectors
RMDEF Vector2 Vector2Lerp(Vector2 v1, Vector2 v2, float amount)
{
Vector2 result = { 0 };
result.x = v1.x + amount*(v2.x - v1.x);
result.y = v1.y + amount*(v2.y - v1.y);
return result;
}
// Rotate Vector by float in Degrees.
RMDEF Vector2 Vector2Rotate(Vector2 v, float degs)
{
float rads = degs*DEG2RAD;
Vector2 result = {v.x * cosf(rads) - v.y * sinf(rads) , v.x * sinf(rads) + v.y * cosf(rads) };
return result;
}
//----------------------------------------------------------------------------------
// Module Functions Definition - Vector3 math
//----------------------------------------------------------------------------------
// Vector with components value 0.0f
RMDEF Vector3 Vector3Zero(void)
{
Vector3 result = { 0.0f, 0.0f, 0.0f };
return result;
}
// Vector with components value 1.0f
RMDEF Vector3 Vector3One(void)
{
Vector3 result = { 1.0f, 1.0f, 1.0f };
return result;
}
// Add two vectors
RMDEF Vector3 Vector3Add(Vector3 v1, Vector3 v2)
{
Vector3 result = { v1.x + v2.x, v1.y + v2.y, v1.z + v2.z };
return result;
}
// Subtract two vectors
RMDEF Vector3 Vector3Subtract(Vector3 v1, Vector3 v2)
{
Vector3 result = { v1.x - v2.x, v1.y - v2.y, v1.z - v2.z };
return result;
}
// Multiply vector by scalar
RMDEF Vector3 Vector3Scale(Vector3 v, float scalar)
{
Vector3 result = { v.x*scalar, v.y*scalar, v.z*scalar };
return result;
}
// Multiply vector by vector
RMDEF Vector3 Vector3Multiply(Vector3 v1, Vector3 v2)
{
Vector3 result = { v1.x*v2.x, v1.y*v2.y, v1.z*v2.z };
return result;
}
// Calculate two vectors cross product
RMDEF Vector3 Vector3CrossProduct(Vector3 v1, Vector3 v2)
{
Vector3 result = { v1.y*v2.z - v1.z*v2.y, v1.z*v2.x - v1.x*v2.z, v1.x*v2.y - v1.y*v2.x };
return result;
}
// Calculate one vector perpendicular vector
RMDEF Vector3 Vector3Perpendicular(Vector3 v)
{
Vector3 result = { 0 };
float min = (float) fabs(v.x);
Vector3 cardinalAxis = {1.0f, 0.0f, 0.0f};
if (fabs(v.y) < min)
{
min = (float) fabs(v.y);
Vector3 tmp = {0.0f, 1.0f, 0.0f};
cardinalAxis = tmp;
}
if (fabs(v.z) < min)
{
Vector3 tmp = {0.0f, 0.0f, 1.0f};
cardinalAxis = tmp;
}
result = Vector3CrossProduct(v, cardinalAxis);
return result;
}
// Calculate vector length
RMDEF float Vector3Length(const Vector3 v)
{
float result = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
return result;
}
// Calculate two vectors dot product
RMDEF float Vector3DotProduct(Vector3 v1, Vector3 v2)
{
float result = (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z);
return result;
}
// Calculate distance between two vectors
RMDEF float Vector3Distance(Vector3 v1, Vector3 v2)
{
float dx = v2.x - v1.x;
float dy = v2.y - v1.y;
float dz = v2.z - v1.z;
float result = sqrtf(dx*dx + dy*dy + dz*dz);
return result;
}
// Negate provided vector (invert direction)
RMDEF Vector3 Vector3Negate(Vector3 v)
{
Vector3 result = { -v.x, -v.y, -v.z };
return result;
}
// Divide vector by a float value
RMDEF Vector3 Vector3Divide(Vector3 v, float div)
{
Vector3 result = { v.x / div, v.y / div, v.z / div };
return result;
}
// Divide vector by vector
RMDEF Vector3 Vector3DivideV(Vector3 v1, Vector3 v2)
{
Vector3 result = { v1.x/v2.x, v1.y/v2.y, v1.z/v2.z };
return result;
}
// Normalize provided vector
RMDEF Vector3 Vector3Normalize(Vector3 v)
{
Vector3 result = v;
float length, ilength;
length = Vector3Length(v);
if (length == 0.0f) length = 1.0f;
ilength = 1.0f/length;
result.x *= ilength;
result.y *= ilength;
result.z *= ilength;
return result;
}
// Orthonormalize provided vectors
// Makes vectors normalized and orthogonal to each other
// Gram-Schmidt function implementation
RMDEF void Vector3OrthoNormalize(Vector3 *v1, Vector3 *v2)
{
*v1 = Vector3Normalize(*v1);
Vector3 vn = Vector3CrossProduct(*v1, *v2);
vn = Vector3Normalize(vn);
*v2 = Vector3CrossProduct(vn, *v1);
}
// Transforms a Vector3 by a given Matrix
RMDEF Vector3 Vector3Transform(Vector3 v, Matrix mat)
{
Vector3 result = { 0 };
float x = v.x;
float y = v.y;
float z = v.z;
result.x = mat.m0*x + mat.m4*y + mat.m8*z + mat.m12;
result.y = mat.m1*x + mat.m5*y + mat.m9*z + mat.m13;
result.z = mat.m2*x + mat.m6*y + mat.m10*z + mat.m14;
return result;
}
// Transform a vector by quaternion rotation
RMDEF Vector3 Vector3RotateByQuaternion(Vector3 v, Quaternion q)
{
Vector3 result = { 0 };
result.x = v.x*(q.x*q.x + q.w*q.w - q.y*q.y - q.z*q.z) + v.y*(2*q.x*q.y - 2*q.w*q.z) + v.z*(2*q.x*q.z + 2*q.w*q.y);
result.y = v.x*(2*q.w*q.z + 2*q.x*q.y) + v.y*(q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z) + v.z*(-2*q.w*q.x + 2*q.y*q.z);
result.z = v.x*(-2*q.w*q.y + 2*q.x*q.z) + v.y*(2*q.w*q.x + 2*q.y*q.z)+ v.z*(q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
return result;
}
// Calculate linear interpolation between two vectors
RMDEF Vector3 Vector3Lerp(Vector3 v1, Vector3 v2, float amount)
{
Vector3 result = { 0 };
result.x = v1.x + amount*(v2.x - v1.x);
result.y = v1.y + amount*(v2.y - v1.y);
result.z = v1.z + amount*(v2.z - v1.z);
return result;
}
// Calculate reflected vector to normal
RMDEF Vector3 Vector3Reflect(Vector3 v, Vector3 normal)
{
// I is the original vector
// N is the normal of the incident plane
// R = I - (2*N*( DotProduct[ I,N] ))
Vector3 result = { 0 };
float dotProduct = Vector3DotProduct(v, normal);
result.x = v.x - (2.0f*normal.x)*dotProduct;
result.y = v.y - (2.0f*normal.y)*dotProduct;
result.z = v.z - (2.0f*normal.z)*dotProduct;
return result;
}
// Return min value for each pair of components
RMDEF Vector3 Vector3Min(Vector3 v1, Vector3 v2)
{
Vector3 result = { 0 };
result.x = fminf(v1.x, v2.x);
result.y = fminf(v1.y, v2.y);
result.z = fminf(v1.z, v2.z);
return result;
}
// Return max value for each pair of components
RMDEF Vector3 Vector3Max(Vector3 v1, Vector3 v2)
{
Vector3 result = { 0 };
result.x = fmaxf(v1.x, v2.x);
result.y = fmaxf(v1.y, v2.y);
result.z = fmaxf(v1.z, v2.z);
return result;
}
// Compute barycenter coordinates (u, v, w) for point p with respect to triangle (a, b, c)
// NOTE: Assumes P is on the plane of the triangle
RMDEF Vector3 Vector3Barycenter(Vector3 p, Vector3 a, Vector3 b, Vector3 c)
{
//Vector v0 = b - a, v1 = c - a, v2 = p - a;
Vector3 v0 = Vector3Subtract(b, a);
Vector3 v1 = Vector3Subtract(c, a);
Vector3 v2 = Vector3Subtract(p, a);
float d00 = Vector3DotProduct(v0, v0);
float d01 = Vector3DotProduct(v0, v1);
float d11 = Vector3DotProduct(v1, v1);
float d20 = Vector3DotProduct(v2, v0);
float d21 = Vector3DotProduct(v2, v1);
float denom = d00*d11 - d01*d01;
Vector3 result = { 0 };
result.y = (d11*d20 - d01*d21)/denom;
result.z = (d00*d21 - d01*d20)/denom;
result.x = 1.0f - (result.z + result.y);
return result;
}
// Returns Vector3 as float array
RMDEF float3 Vector3ToFloatV(Vector3 v)
{
float3 buffer = { 0 };
buffer.v[0] = v.x;
buffer.v[1] = v.y;
buffer.v[2] = v.z;
return buffer;
}
//----------------------------------------------------------------------------------
// Module Functions Definition - Matrix math
//----------------------------------------------------------------------------------
// Compute matrix determinant
RMDEF float MatrixDeterminant(Matrix mat)
{
// Cache the matrix values (speed optimization)
float a00 = mat.m0, a01 = mat.m1, a02 = mat.m2, a03 = mat.m3;
float a10 = mat.m4, a11 = mat.m5, a12 = mat.m6, a13 = mat.m7;
float a20 = mat.m8, a21 = mat.m9, a22 = mat.m10, a23 = mat.m11;
float a30 = mat.m12, a31 = mat.m13, a32 = mat.m14, a33 = mat.m15;
float result = a30*a21*a12*a03 - a20*a31*a12*a03 - a30*a11*a22*a03 + a10*a31*a22*a03 +
a20*a11*a32*a03 - a10*a21*a32*a03 - a30*a21*a02*a13 + a20*a31*a02*a13 +
a30*a01*a22*a13 - a00*a31*a22*a13 - a20*a01*a32*a13 + a00*a21*a32*a13 +
a30*a11*a02*a23 - a10*a31*a02*a23 - a30*a01*a12*a23 + a00*a31*a12*a23 +
a10*a01*a32*a23 - a00*a11*a32*a23 - a20*a11*a02*a33 + a10*a21*a02*a33 +
a20*a01*a12*a33 - a00*a21*a12*a33 - a10*a01*a22*a33 + a00*a11*a22*a33;
return result;
}
// Returns the trace of the matrix (sum of the values along the diagonal)
RMDEF float MatrixTrace(Matrix mat)
{
float result = (mat.m0 + mat.m5 + mat.m10 + mat.m15);
return result;
}
// Transposes provided matrix
RMDEF Matrix MatrixTranspose(Matrix mat)
{
Matrix result = { 0 };
result.m0 = mat.m0;
result.m1 = mat.m4;
result.m2 = mat.m8;
result.m3 = mat.m12;
result.m4 = mat.m1;
result.m5 = mat.m5;
result.m6 = mat.m9;
result.m7 = mat.m13;
result.m8 = mat.m2;
result.m9 = mat.m6;
result.m10 = mat.m10;
result.m11 = mat.m14;
result.m12 = mat.m3;
result.m13 = mat.m7;
result.m14 = mat.m11;
result.m15 = mat.m15;
return result;
}
// Invert provided matrix
RMDEF Matrix MatrixInvert(Matrix mat)
{
Matrix result = { 0 };
// Cache the matrix values (speed optimization)
float a00 = mat.m0, a01 = mat.m1, a02 = mat.m2, a03 = mat.m3;
float a10 = mat.m4, a11 = mat.m5, a12 = mat.m6, a13 = mat.m7;
float a20 = mat.m8, a21 = mat.m9, a22 = mat.m10, a23 = mat.m11;
float a30 = mat.m12, a31 = mat.m13, a32 = mat.m14, a33 = mat.m15;
float b00 = a00*a11 - a01*a10;
float b01 = a00*a12 - a02*a10;
float b02 = a00*a13 - a03*a10;
float b03 = a01*a12 - a02*a11;
float b04 = a01*a13 - a03*a11;
float b05 = a02*a13 - a03*a12;
float b06 = a20*a31 - a21*a30;
float b07 = a20*a32 - a22*a30;
float b08 = a20*a33 - a23*a30;
float b09 = a21*a32 - a22*a31;
float b10 = a21*a33 - a23*a31;
float b11 = a22*a33 - a23*a32;
// Calculate the invert determinant (inlined to avoid double-caching)
float invDet = 1.0f/(b00*b11 - b01*b10 + b02*b09 + b03*b08 - b04*b07 + b05*b06);
result.m0 = (a11*b11 - a12*b10 + a13*b09)*invDet;
result.m1 = (-a01*b11 + a02*b10 - a03*b09)*invDet;
result.m2 = (a31*b05 - a32*b04 + a33*b03)*invDet;
result.m3 = (-a21*b05 + a22*b04 - a23*b03)*invDet;
result.m4 = (-a10*b11 + a12*b08 - a13*b07)*invDet;
result.m5 = (a00*b11 - a02*b08 + a03*b07)*invDet;
result.m6 = (-a30*b05 + a32*b02 - a33*b01)*invDet;
result.m7 = (a20*b05 - a22*b02 + a23*b01)*invDet;
result.m8 = (a10*b10 - a11*b08 + a13*b06)*invDet;
result.m9 = (-a00*b10 + a01*b08 - a03*b06)*invDet;
result.m10 = (a30*b04 - a31*b02 + a33*b00)*invDet;
result.m11 = (-a20*b04 + a21*b02 - a23*b00)*invDet;
result.m12 = (-a10*b09 + a11*b07 - a12*b06)*invDet;
result.m13 = (a00*b09 - a01*b07 + a02*b06)*invDet;
result.m14 = (-a30*b03 + a31*b01 - a32*b00)*invDet;
result.m15 = (a20*b03 - a21*b01 + a22*b00)*invDet;
return result;
}
// Normalize provided matrix
RMDEF Matrix MatrixNormalize(Matrix mat)
{
Matrix result = { 0 };
float det = MatrixDeterminant(mat);
result.m0 = mat.m0/det;
result.m1 = mat.m1/det;
result.m2 = mat.m2/det;
result.m3 = mat.m3/det;
result.m4 = mat.m4/det;
result.m5 = mat.m5/det;
result.m6 = mat.m6/det;
result.m7 = mat.m7/det;
result.m8 = mat.m8/det;
result.m9 = mat.m9/det;
result.m10 = mat.m10/det;
result.m11 = mat.m11/det;
result.m12 = mat.m12/det;
result.m13 = mat.m13/det;
result.m14 = mat.m14/det;
result.m15 = mat.m15/det;
return result;
}
// Returns identity matrix
RMDEF Matrix MatrixIdentity(void)
{
Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f };
return result;
}
// Add two matrices
RMDEF Matrix MatrixAdd(Matrix left, Matrix right)
{
Matrix result = MatrixIdentity();
result.m0 = left.m0 + right.m0;
result.m1 = left.m1 + right.m1;
result.m2 = left.m2 + right.m2;
result.m3 = left.m3 + right.m3;
result.m4 = left.m4 + right.m4;
result.m5 = left.m5 + right.m5;
result.m6 = left.m6 + right.m6;
result.m7 = left.m7 + right.m7;
result.m8 = left.m8 + right.m8;
result.m9 = left.m9 + right.m9;
result.m10 = left.m10 + right.m10;
result.m11 = left.m11 + right.m11;
result.m12 = left.m12 + right.m12;
result.m13 = left.m13 + right.m13;
result.m14 = left.m14 + right.m14;
result.m15 = left.m15 + right.m15;
return result;
}
// Subtract two matrices (left - right)
RMDEF Matrix MatrixSubtract(Matrix left, Matrix right)
{
Matrix result = MatrixIdentity();
result.m0 = left.m0 - right.m0;
result.m1 = left.m1 - right.m1;
result.m2 = left.m2 - right.m2;
result.m3 = left.m3 - right.m3;
result.m4 = left.m4 - right.m4;
result.m5 = left.m5 - right.m5;
result.m6 = left.m6 - right.m6;
result.m7 = left.m7 - right.m7;
result.m8 = left.m8 - right.m8;
result.m9 = left.m9 - right.m9;
result.m10 = left.m10 - right.m10;
result.m11 = left.m11 - right.m11;
result.m12 = left.m12 - right.m12;
result.m13 = left.m13 - right.m13;
result.m14 = left.m14 - right.m14;
result.m15 = left.m15 - right.m15;
return result;
}
// Returns translation matrix
RMDEF Matrix MatrixTranslate(float x, float y, float z)
{
Matrix result = { 1.0f, 0.0f, 0.0f, x,
0.0f, 1.0f, 0.0f, y,
0.0f, 0.0f, 1.0f, z,
0.0f, 0.0f, 0.0f, 1.0f };
return result;
}
// Create rotation matrix from axis and angle
// NOTE: Angle should be provided in radians
RMDEF Matrix MatrixRotate(Vector3 axis, float angle)
{
Matrix result = { 0 };
float x = axis.x, y = axis.y, z = axis.z;
float length = sqrtf(x*x + y*y + z*z);
if ((length != 1.0f) && (length != 0.0f))
{
length = 1.0f/length;
x *= length;
y *= length;
z *= length;
}
float sinres = sinf(angle);
float cosres = cosf(angle);
float t = 1.0f - cosres;
result.m0 = x*x*t + cosres;
result.m1 = y*x*t + z*sinres;
result.m2 = z*x*t - y*sinres;
result.m3 = 0.0f;
result.m4 = x*y*t - z*sinres;
result.m5 = y*y*t + cosres;
result.m6 = z*y*t + x*sinres;
result.m7 = 0.0f;
result.m8 = x*z*t + y*sinres;
result.m9 = y*z*t - x*sinres;
result.m10 = z*z*t + cosres;
result.m11 = 0.0f;
result.m12 = 0.0f;
result.m13 = 0.0f;
result.m14 = 0.0f;
result.m15 = 1.0f;
return result;
}
// Returns xyz-rotation matrix (angles in radians)
RMDEF Matrix MatrixRotateXYZ(Vector3 ang)
{
Matrix result = MatrixIdentity();
float cosz = cosf(-ang.z);
float sinz = sinf(-ang.z);
float cosy = cosf(-ang.y);
float siny = sinf(-ang.y);
float cosx = cosf(-ang.x);
float sinx = sinf(-ang.x);
result.m0 = cosz * cosy;
result.m4 = (cosz * siny * sinx) - (sinz * cosx);
result.m8 = (cosz * siny * cosx) + (sinz * sinx);
result.m1 = sinz * cosy;
result.m5 = (sinz * siny * sinx) + (cosz * cosx);
result.m9 = (sinz * siny * cosx) - (cosz * sinx);
result.m2 = -siny;
result.m6 = cosy * sinx;
result.m10= cosy * cosx;
return result;
}
// Returns x-rotation matrix (angle in radians)
RMDEF Matrix MatrixRotateX(float angle)
{
Matrix result = MatrixIdentity();
float cosres = cosf(angle);
float sinres = sinf(angle);
result.m5 = cosres;
result.m6 = -sinres;
result.m9 = sinres;
result.m10 = cosres;
return result;
}
// Returns y-rotation matrix (angle in radians)
RMDEF Matrix MatrixRotateY(float angle)
{
Matrix result = MatrixIdentity();
float cosres = cosf(angle);
float sinres = sinf(angle);
result.m0 = cosres;
result.m2 = sinres;
result.m8 = -sinres;
result.m10 = cosres;
return result;
}
// Returns z-rotation matrix (angle in radians)
RMDEF Matrix MatrixRotateZ(float angle)
{
Matrix result = MatrixIdentity();
float cosres = cosf(angle);
float sinres = sinf(angle);
result.m0 = cosres;
result.m1 = -sinres;
result.m4 = sinres;
result.m5 = cosres;
return result;
}
// Returns scaling matrix
RMDEF Matrix MatrixScale(float x, float y, float z)
{
Matrix result = { x, 0.0f, 0.0f, 0.0f,
0.0f, y, 0.0f, 0.0f,
0.0f, 0.0f, z, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f };
return result;
}
// Returns two matrix multiplication
// NOTE: When multiplying matrices... the order matters!
RMDEF Matrix MatrixMultiply(Matrix left, Matrix right)
{
Matrix result = { 0 };
result.m0 = left.m0*right.m0 + left.m1*right.m4 + left.m2*right.m8 + left.m3*right.m12;
result.m1 = left.m0*right.m1 + left.m1*right.m5 + left.m2*right.m9 + left.m3*right.m13;
result.m2 = left.m0*right.m2 + left.m1*right.m6 + left.m2*right.m10 + left.m3*right.m14;
result.m3 = left.m0*right.m3 + left.m1*right.m7 + left.m2*right.m11 + left.m3*right.m15;
result.m4 = left.m4*right.m0 + left.m5*right.m4 + left.m6*right.m8 + left.m7*right.m12;
result.m5 = left.m4*right.m1 + left.m5*right.m5 + left.m6*right.m9 + left.m7*right.m13;
result.m6 = left.m4*right.m2 + left.m5*right.m6 + left.m6*right.m10 + left.m7*right.m14;
result.m7 = left.m4*right.m3 + left.m5*right.m7 + left.m6*right.m11 + left.m7*right.m15;
result.m8 = left.m8*right.m0 + left.m9*right.m4 + left.m10*right.m8 + left.m11*right.m12;
result.m9 = left.m8*right.m1 + left.m9*right.m5 + left.m10*right.m9 + left.m11*right.m13;
result.m10 = left.m8*right.m2 + left.m9*right.m6 + left.m10*right.m10 + left.m11*right.m14;
result.m11 = left.m8*right.m3 + left.m9*right.m7 + left.m10*right.m11 + left.m11*right.m15;
result.m12 = left.m12*right.m0 + left.m13*right.m4 + left.m14*right.m8 + left.m15*right.m12;
result.m13 = left.m12*right.m1 + left.m13*right.m5 + left.m14*right.m9 + left.m15*right.m13;
result.m14 = left.m12*right.m2 + left.m13*right.m6 + left.m14*right.m10 + left.m15*right.m14;
result.m15 = left.m12*right.m3 + left.m13*right.m7 + left.m14*right.m11 + left.m15*right.m15;
return result;
}
// Returns perspective projection matrix
RMDEF Matrix MatrixFrustum(double left, double right, double bottom, double top, double near, double far)
{
Matrix result = { 0 };
float rl = (float)(right - left);
float tb = (float)(top - bottom);
float fn = (float)(far - near);
result.m0 = ((float) near*2.0f)/rl;
result.m1 = 0.0f;
result.m2 = 0.0f;
result.m3 = 0.0f;
result.m4 = 0.0f;
result.m5 = ((float) near*2.0f)/tb;
result.m6 = 0.0f;
result.m7 = 0.0f;
result.m8 = ((float)right + (float)left)/rl;
result.m9 = ((float)top + (float)bottom)/tb;
result.m10 = -((float)far + (float)near)/fn;
result.m11 = -1.0f;
result.m12 = 0.0f;
result.m13 = 0.0f;
result.m14 = -((float)far*(float)near*2.0f)/fn;
result.m15 = 0.0f;
return result;
}
// Returns perspective projection matrix
// NOTE: Angle should be provided in radians
RMDEF Matrix MatrixPerspective(double fovy, double aspect, double near, double far)
{
double top = near*tan(fovy*0.5);
double right = top*aspect;
Matrix result = MatrixFrustum(-right, right, -top, top, near, far);
return result;
}
// Returns orthographic projection matrix
RMDEF Matrix MatrixOrtho(double left, double right, double bottom, double top, double near, double far)
{
Matrix result = { 0 };
float rl = (float)(right - left);
float tb = (float)(top - bottom);
float fn = (float)(far - near);
result.m0 = 2.0f/rl;
result.m1 = 0.0f;
result.m2 = 0.0f;
result.m3 = 0.0f;
result.m4 = 0.0f;
result.m5 = 2.0f/tb;
result.m6 = 0.0f;
result.m7 = 0.0f;
result.m8 = 0.0f;
result.m9 = 0.0f;
result.m10 = -2.0f/fn;
result.m11 = 0.0f;
result.m12 = -((float)left + (float)right)/rl;
result.m13 = -((float)top + (float)bottom)/tb;
result.m14 = -((float)far + (float)near)/fn;
result.m15 = 1.0f;
return result;
}
// Returns camera look-at matrix (view matrix)
RMDEF Matrix MatrixLookAt(Vector3 eye, Vector3 target, Vector3 up)
{
Matrix result = { 0 };
Vector3 z = Vector3Subtract(eye, target);
z = Vector3Normalize(z);
Vector3 x = Vector3CrossProduct(up, z);
x = Vector3Normalize(x);
Vector3 y = Vector3CrossProduct(z, x);
y = Vector3Normalize(y);
result.m0 = x.x;
result.m1 = x.y;
result.m2 = x.z;
result.m3 = 0.0f;
result.m4 = y.x;
result.m5 = y.y;
result.m6 = y.z;
result.m7 = 0.0f;
result.m8 = z.x;
result.m9 = z.y;
result.m10 = z.z;
result.m11 = 0.0f;
result.m12 = eye.x;
result.m13 = eye.y;
result.m14 = eye.z;
result.m15 = 1.0f;
result = MatrixInvert(result);
return result;
}
// Returns float array of matrix data
RMDEF float16 MatrixToFloatV(Matrix mat)
{
float16 buffer = { 0 };
buffer.v[0] = mat.m0;
buffer.v[1] = mat.m1;
buffer.v[2] = mat.m2;
buffer.v[3] = mat.m3;
buffer.v[4] = mat.m4;
buffer.v[5] = mat.m5;
buffer.v[6] = mat.m6;
buffer.v[7] = mat.m7;
buffer.v[8] = mat.m8;
buffer.v[9] = mat.m9;
buffer.v[10] = mat.m10;
buffer.v[11] = mat.m11;
buffer.v[12] = mat.m12;
buffer.v[13] = mat.m13;
buffer.v[14] = mat.m14;
buffer.v[15] = mat.m15;
return buffer;
}
//----------------------------------------------------------------------------------
// Module Functions Definition - Quaternion math
//----------------------------------------------------------------------------------
// Returns identity quaternion
RMDEF Quaternion QuaternionIdentity(void)
{
Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f };
return result;
}
// Computes the length of a quaternion
RMDEF float QuaternionLength(Quaternion q)
{
float result = (float)sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
return result;
}
// Normalize provided quaternion
RMDEF Quaternion QuaternionNormalize(Quaternion q)
{
Quaternion result = { 0 };
float length, ilength;
length = QuaternionLength(q);
if (length == 0.0f) length = 1.0f;
ilength = 1.0f/length;
result.x = q.x*ilength;
result.y = q.y*ilength;
result.z = q.z*ilength;
result.w = q.w*ilength;
return result;
}
// Invert provided quaternion
RMDEF Quaternion QuaternionInvert(Quaternion q)
{
Quaternion result = q;
float length = QuaternionLength(q);
float lengthSq = length*length;
if (lengthSq != 0.0)
{
float i = 1.0f/lengthSq;
result.x *= -i;
result.y *= -i;
result.z *= -i;
result.w *= i;
}
return result;
}
// Calculate two quaternion multiplication
RMDEF Quaternion QuaternionMultiply(Quaternion q1, Quaternion q2)
{
Quaternion result = { 0 };
float qax = q1.x, qay = q1.y, qaz = q1.z, qaw = q1.w;
float qbx = q2.x, qby = q2.y, qbz = q2.z, qbw = q2.w;
result.x = qax*qbw + qaw*qbx + qay*qbz - qaz*qby;
result.y = qay*qbw + qaw*qby + qaz*qbx - qax*qbz;
result.z = qaz*qbw + qaw*qbz + qax*qby - qay*qbx;
result.w = qaw*qbw - qax*qbx - qay*qby - qaz*qbz;
return result;
}
// Calculate linear interpolation between two quaternions
RMDEF Quaternion QuaternionLerp(Quaternion q1, Quaternion q2, float amount)
{
Quaternion result = { 0 };
result.x = q1.x + amount*(q2.x - q1.x);
result.y = q1.y + amount*(q2.y - q1.y);
result.z = q1.z + amount*(q2.z - q1.z);
result.w = q1.w + amount*(q2.w - q1.w);
return result;
}
// Calculate slerp-optimized interpolation between two quaternions
RMDEF Quaternion QuaternionNlerp(Quaternion q1, Quaternion q2, float amount)
{
Quaternion result = QuaternionLerp(q1, q2, amount);
result = QuaternionNormalize(result);
return result;
}
// Calculates spherical linear interpolation between two quaternions
RMDEF Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float amount)
{
Quaternion result = { 0 };
float cosHalfTheta = q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
if (fabs(cosHalfTheta) >= 1.0f) result = q1;
else if (cosHalfTheta > 0.95f) result = QuaternionNlerp(q1, q2, amount);
else
{
float halfTheta = acosf(cosHalfTheta);
float sinHalfTheta = sqrtf(1.0f - cosHalfTheta*cosHalfTheta);
if (fabs(sinHalfTheta) < 0.001f)
{
result.x = (q1.x*0.5f + q2.x*0.5f);
result.y = (q1.y*0.5f + q2.y*0.5f);
result.z = (q1.z*0.5f + q2.z*0.5f);
result.w = (q1.w*0.5f + q2.w*0.5f);
}
else
{
float ratioA = sinf((1 - amount)*halfTheta)/sinHalfTheta;
float ratioB = sinf(amount*halfTheta)/sinHalfTheta;
result.x = (q1.x*ratioA + q2.x*ratioB);
result.y = (q1.y*ratioA + q2.y*ratioB);
result.z = (q1.z*ratioA + q2.z*ratioB);
result.w = (q1.w*ratioA + q2.w*ratioB);
}
}
return result;
}
// Calculate quaternion based on the rotation from one vector to another
RMDEF Quaternion QuaternionFromVector3ToVector3(Vector3 from, Vector3 to)
{
Quaternion result = { 0 };
float cos2Theta = Vector3DotProduct(from, to);
Vector3 cross = Vector3CrossProduct(from, to);
result.x = cross.x;
result.y = cross.y;
result.z = cross.y;
result.w = 1.0f + cos2Theta; // NOTE: Added QuaternioIdentity()
// Normalize to essentially nlerp the original and identity to 0.5
result = QuaternionNormalize(result);
// Above lines are equivalent to:
//Quaternion result = QuaternionNlerp(q, QuaternionIdentity(), 0.5f);
return result;
}
// Returns a quaternion for a given rotation matrix
RMDEF Quaternion QuaternionFromMatrix(Matrix mat)
{
Quaternion result = { 0 };
float trace = MatrixTrace(mat);
if (trace > 0.0f)
{
float s = sqrtf(trace + 1)*2.0f;
float invS = 1.0f/s;
result.w = s*0.25f;
result.x = (mat.m6 - mat.m9)*invS;
result.y = (mat.m8 - mat.m2)*invS;
result.z = (mat.m1 - mat.m4)*invS;
}
else
{
float m00 = mat.m0, m11 = mat.m5, m22 = mat.m10;
if (m00 > m11 && m00 > m22)
{
float s = (float)sqrt(1.0f + m00 - m11 - m22)*2.0f;
float invS = 1.0f/s;
result.w = (mat.m6 - mat.m9)*invS;
result.x = s*0.25f;
result.y = (mat.m4 + mat.m1)*invS;
result.z = (mat.m8 + mat.m2)*invS;
}
else if (m11 > m22)
{
float s = sqrtf(1.0f + m11 - m00 - m22)*2.0f;
float invS = 1.0f/s;
result.w = (mat.m8 - mat.m2)*invS;
result.x = (mat.m4 + mat.m1)*invS;
result.y = s*0.25f;
result.z = (mat.m9 + mat.m6)*invS;
}
else
{
float s = sqrtf(1.0f + m22 - m00 - m11)*2.0f;
float invS = 1.0f/s;
result.w = (mat.m1 - mat.m4)*invS;
result.x = (mat.m8 + mat.m2)*invS;
result.y = (mat.m9 + mat.m6)*invS;
result.z = s*0.25f;
}
}
return result;
}
// Returns a matrix for a given quaternion
RMDEF Matrix QuaternionToMatrix(Quaternion q)
{
Matrix result = { 0 };
float x = q.x, y = q.y, z = q.z, w = q.w;
float x2 = x + x;
float y2 = y + y;
float z2 = z + z;
float length = QuaternionLength(q);
float lengthSquared = length*length;
float xx = x*x2/lengthSquared;
float xy = x*y2/lengthSquared;
float xz = x*z2/lengthSquared;
float yy = y*y2/lengthSquared;
float yz = y*z2/lengthSquared;
float zz = z*z2/lengthSquared;
float wx = w*x2/lengthSquared;
float wy = w*y2/lengthSquared;
float wz = w*z2/lengthSquared;
result.m0 = 1.0f - (yy + zz);
result.m1 = xy - wz;
result.m2 = xz + wy;
result.m3 = 0.0f;
result.m4 = xy + wz;
result.m5 = 1.0f - (xx + zz);
result.m6 = yz - wx;
result.m7 = 0.0f;
result.m8 = xz - wy;
result.m9 = yz + wx;
result.m10 = 1.0f - (xx + yy);
result.m11 = 0.0f;
result.m12 = 0.0f;
result.m13 = 0.0f;
result.m14 = 0.0f;
result.m15 = 1.0f;
return result;
}
// Returns rotation quaternion for an angle and axis
// NOTE: angle must be provided in radians
RMDEF Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle)
{
Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f };
if (Vector3Length(axis) != 0.0f)
angle *= 0.5f;
axis = Vector3Normalize(axis);
float sinres = sinf(angle);
float cosres = cosf(angle);
result.x = axis.x*sinres;
result.y = axis.y*sinres;
result.z = axis.z*sinres;
result.w = cosres;
result = QuaternionNormalize(result);
return result;
}
// Returns the rotation angle and axis for a given quaternion
RMDEF void QuaternionToAxisAngle(Quaternion q, Vector3 *outAxis, float *outAngle)
{
if (fabs(q.w) > 1.0f) q = QuaternionNormalize(q);
Vector3 resAxis = { 0.0f, 0.0f, 0.0f };
float resAngle = 2.0f*acosf(q.w);
float den = sqrtf(1.0f - q.w*q.w);
if (den > 0.0001f)
{
resAxis.x = q.x/den;
resAxis.y = q.y/den;
resAxis.z = q.z/den;
}
else
{
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
resAxis.x = 1.0f;
}
*outAxis = resAxis;
*outAngle = resAngle;
}
// Returns he quaternion equivalent to Euler angles
RMDEF Quaternion QuaternionFromEuler(float roll, float pitch, float yaw)
{
Quaternion q = { 0 };
float x0 = cosf(roll*0.5f);
float x1 = sinf(roll*0.5f);
float y0 = cosf(pitch*0.5f);
float y1 = sinf(pitch*0.5f);
float z0 = cosf(yaw*0.5f);
float z1 = sinf(yaw*0.5f);
q.x = x1*y0*z0 - x0*y1*z1;
q.y = x0*y1*z0 + x1*y0*z1;
q.z = x0*y0*z1 - x1*y1*z0;
q.w = x0*y0*z0 + x1*y1*z1;
return q;
}
// Return the Euler angles equivalent to quaternion (roll, pitch, yaw)
// NOTE: Angles are returned in a Vector3 struct in degrees
RMDEF Vector3 QuaternionToEuler(Quaternion q)
{
Vector3 result = { 0 };
// roll (x-axis rotation)
float x0 = 2.0f*(q.w*q.x + q.y*q.z);
float x1 = 1.0f - 2.0f*(q.x*q.x + q.y*q.y);
result.x = atan2f(x0, x1)*RAD2DEG;
// pitch (y-axis rotation)
float y0 = 2.0f*(q.w*q.y - q.z*q.x);
y0 = y0 > 1.0f ? 1.0f : y0;
y0 = y0 < -1.0f ? -1.0f : y0;
result.y = asinf(y0)*RAD2DEG;
// yaw (z-axis rotation)
float z0 = 2.0f*(q.w*q.z + q.x*q.y);
float z1 = 1.0f - 2.0f*(q.y*q.y + q.z*q.z);
result.z = atan2f(z0, z1)*RAD2DEG;
return result;
}
// Transform a quaternion given a transformation matrix
RMDEF Quaternion QuaternionTransform(Quaternion q, Matrix mat)
{
Quaternion result = { 0 };
result.x = mat.m0*q.x + mat.m4*q.y + mat.m8*q.z + mat.m12*q.w;
result.y = mat.m1*q.x + mat.m5*q.y + mat.m9*q.z + mat.m13*q.w;
result.z = mat.m2*q.x + mat.m6*q.y + mat.m10*q.z + mat.m14*q.w;
result.w = mat.m3*q.x + mat.m7*q.y + mat.m11*q.z + mat.m15*q.w;
return result;
}
#endif // RAYMATH_H
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment