From 35b94829281e8e2b3138cbcc8c861e067da59eec Mon Sep 17 00:00:00 2001 From: John Alanbrook Date: Sat, 28 Jan 2023 04:35:08 +0000 Subject: [PATCH] Tiny physics engine --- source/engine/thirdparty/tinyphysicsengine.h | 3303 ++++++++++++++++++ 1 file changed, 3303 insertions(+) create mode 100644 source/engine/thirdparty/tinyphysicsengine.h diff --git a/source/engine/thirdparty/tinyphysicsengine.h b/source/engine/thirdparty/tinyphysicsengine.h new file mode 100644 index 0000000..ff0e8c1 --- /dev/null +++ b/source/engine/thirdparty/tinyphysicsengine.h @@ -0,0 +1,3303 @@ +#ifndef _TINYPHYSICSENGINE_H +#define _TINYPHYSICSENGINE_H + +/** + tinyphysicsengine (TPE) + + Simple/suckless header-only hybrid 3D physics engine with no floating point, + only 32 bit int arithmetic, similar to e.g. small3dlib. + + Conventions and formats are the same or similar to those of small3dlib so as + to make them easily integrate with each other. + + The library works with bodies made of spheres connected by elastic springs, + i.e. soft bodies which however behave as "stiff" bodies by default and can + be used to fake rigid body physics as well. Bodies are placed in environemnts + specified by a distance function that allows to implement any mathematical + shape. + + Orientations/rotations are in extrinsic Euler angles in the ZXY order (by Z, + then by X, then by Y), if not mentioned otherwise. Angles are in TPE_Units, + TPE_FRACTIONS_PER_UNIT is full angle (2 PI). Sometimes rotations can also be + specified in the "about axis" format: here the object is rotated CW by given + axis by an angle that's specified by the magnitude of the vector. + + Where it matters (e.g. rotations about axes) we consider a left-handed coord. + system (x right, y up, z forward). + + ------------------------------------------------------------------------------ + + by drummyfish, 2022 + + version 0.8d + + This work's goal is to never be encumbered by any exclusive intellectual + property rights. The work is therefore provided under CC0 1.0 + (https://creativecommons.org/publicdomain/zero/1.0/) + additional WAIVER OF + ALL INTELLECTUAL PROPERTY RIGHTS that waives the rest of intellectual property + rights not already waived by CC0 1.0. The WAIVER OF ALL INTELLECTUAL PROPERTY + RGHTS is as follows: + + Each contributor to this work agrees that they waive any exclusive rights, + including but not limited to copyright, patents, trademark, trade dress, + industrial design, plant varieties and trade secrets, to any and all ideas, + concepts, processes, discoveries, improvements and inventions conceived, + discovered, made, designed, researched or developed by the contributor either + solely or jointly with others, which relate to this work or result from this + work. Should any waiver of such right be judged legally invalid or + ineffective under applicable law, the contributor hereby grants to each + affected person a royalty-free, non transferable, non sublicensable, non + exclusive, irrevocable and unconditional license to this right. +*/ + +#include + +typedef int32_t TPE_Unit; ///< Basic fixed point unit type. +typedef int16_t TPE_UnitReduced; ///< Like TPE_Unit but saving space + +#define TPE_FRACTIONS_PER_UNIT 512 ///< one fixed point unit, don't change +#define TPE_F TPE_FRACTIONS_PER_UNIT ///< short for TPE_FRACTIONS_PER_UNIT +#define TPE_JOINT_SIZE_MULTIPLIER 32 ///< joint size is scaled (size saving) + +#define TPE_INFINITY 2147483647 + +#define TPE_JOINT_SIZE(joint) ((joint).sizeDivided * TPE_JOINT_SIZE_MULTIPLIER) + +#ifndef TPE_APPROXIMATE_LENGTH + #define TPE_APPROXIMATE_LENGTH 0 /**< whether or not use length/distance + approximation rather than exact + calculation (1 is faster but less + accurate), beware of possible lower + stability */ +#endif + +#if !TPE_APPROXIMATE_LENGTH + #define TPE_DISTANCE TPE_dist + #define TPE_LENGTH TPE_vec3Len +#else + #define TPE_DISTANCE TPE_distApprox + #define TPE_LENGTH TPE_vec3LenApprox +#endif + +#ifndef TPE_LOG + #define TPE_LOG(s) ; // redefine to some print function to show debug logs +#endif + +#ifndef TPE_LOW_SPEED +/** Speed, in TPE_Units per ticks, that is considered low (used e.g. for auto + deactivation of bodies). */ + #define TPE_LOW_SPEED 30 +#endif + +#ifndef TPE_RESHAPE_TENSION_LIMIT +/** Tension limit, in TPE_Units, after which a non-soft body will be reshaped. + Smaller number will keep more stable shapes but will cost more performance. */ + #define TPE_RESHAPE_TENSION_LIMIT 20 +#endif + +#ifndef TPE_RESHAPE_ITERATIONS +/** How many iterations of reshaping will be performed by the step function if + the body's shape needs to be reshaped. Greater number will keep shapes more + stable but will cost some performance. */ + #define TPE_RESHAPE_ITERATIONS 3 +#endif + +#ifndef TPE_DEACTIVATE_AFTER +/** After how many ticks of low speed should a body be disabled. This mustn't + be greater than 255. */ + #define TPE_DEACTIVATE_AFTER 128 +#endif + +#ifndef TPE_LIGHT_DEACTIVATION +/** When a body is activated by a collision, its deactivation counter will be + set to this value, i.e. after a collision the body will be prone to deactivate + sooner than normally. This is to handle situations with many bodies touching + each other that would normally keep activating each other, never coming to + rest. */ + #define TPE_LIGHT_DEACTIVATION \ + (TPE_DEACTIVATE_AFTER - TPE_DEACTIVATE_AFTER / 10) +#endif + +#ifndef TPE_TENSION_ACCELERATION_DIVIDER +/** Number by which the base acceleration (TPE_FRACTIONS_PER_UNIT per tick + squared) caused by the connection tension will be divided. This should be + power of 2. */ + #define TPE_TENSION_ACCELERATION_DIVIDER 32 +#endif + +#ifndef TPE_TENSION_ACCELERATION_THRESHOLD +/** Limit within which acceleration caused by connection tension won't be + applied. */ + #define TPE_TENSION_ACCELERATION_THRESHOLD 5 +#endif + +#ifndef TPE_TENSION_GREATER_ACCELERATION_THRESHOLD +/** Connection tension threshold after which twice as much acceleration will + be applied. This helps prevent diverting joints that are "impaled" by + environment.*/ + #define TPE_TENSION_GREATER_ACCELERATION_THRESHOLD \ + (TPE_TENSION_ACCELERATION_THRESHOLD * 3) +#endif + +#ifndef TPE_COLLISION_RESOLUTION_ITERATIONS +/** Maximum number of iterations to try to uncollide two colliding bodies. */ + #define TPE_COLLISION_RESOLUTION_ITERATIONS 16 +#endif + +#ifndef TPE_COLLISION_RESOLUTION_MARGIN +/** Margin, in TPE_Units, by which a body will be shifted back to get out of + collision. */ + #define TPE_COLLISION_RESOLUTION_MARGIN (TPE_F / 64) +#endif + +#ifndef TPE_NONROTATING_COLLISION_RESOLVE_ATTEMPTS +/** Number of times a collision of nonrotating bodies with environment will be + attempted to resolve. This probably won't have great performance implications + as complex collisions of this kind should be relatively rare. */ + #define TPE_NONROTATING_COLLISION_RESOLVE_ATTEMPTS 8 +#endif + +#ifndef TPE_APPROXIMATE_NET_SPEED +/** Whether to use a fast approximation for calculating net speed of bodies + which increases performance a bit. */ + #define TPE_APPROXIMATE_NET_SPEED 1 +#endif + +#define TPE_PRINTF_VEC3(v) printf("[%d %d %d]",(v).x,(v).y,(v).z); + +typedef struct +{ + TPE_Unit x; + TPE_Unit y; + TPE_Unit z; +} TPE_Vec3; + +/** Keeps given point within specified axis-aligned box. This can be used e.g. + to smooth rendered movement of jittering physics bodies. */ +TPE_Vec3 TPE_vec3KeepWithinBox(TPE_Vec3 point, TPE_Vec3 boxCenter, + TPE_Vec3 boxMaxVect); + +TPE_Vec3 TPE_vec3KeepWithinDistanceBand(TPE_Vec3 point, TPE_Vec3 center, + TPE_Unit minDistance, TPE_Unit maxDistance); + +TPE_Vec3 TPE_vec3(TPE_Unit x, TPE_Unit y, TPE_Unit z); +TPE_Vec3 TPE_vec3Minus(TPE_Vec3 v1, TPE_Vec3 v2); +TPE_Vec3 TPE_vec3Plus(TPE_Vec3 v1, TPE_Vec3 v2); +TPE_Vec3 TPE_vec3Cross(TPE_Vec3 v1, TPE_Vec3 v2); +TPE_Vec3 TPE_vec3Project(TPE_Vec3 v, TPE_Vec3 base); +TPE_Vec3 TPE_vec3ProjectNormalized(TPE_Vec3 v, TPE_Vec3 baseNormalized); +TPE_Vec3 TPE_vec3Times(TPE_Vec3 v, TPE_Unit units); +TPE_Vec3 TPE_vec3TimesPlain(TPE_Vec3 v, TPE_Unit q); +TPE_Vec3 TPE_vec3Normalized(TPE_Vec3 v); + +TPE_Unit TPE_vec3Dot(TPE_Vec3 v1, TPE_Vec3 v2); +TPE_Unit TPE_vec3Len(TPE_Vec3 v); +TPE_Unit TPE_vec3LenApprox(TPE_Vec3 v); + +/** Returns an angle in TPE_Units (see angle conventions) of a 2D vector with + the X axis, CCW. */ +TPE_Unit TPE_vec2Angle(TPE_Unit x, TPE_Unit y); + +/** Keeps given value within specified range. This can be used e.g. for movement + smoothing. */ +TPE_Unit TPE_keepInRange(TPE_Unit x, TPE_Unit xMin, TPE_Unit xMax); + +static inline TPE_Unit TPE_abs(TPE_Unit x); +static inline TPE_Unit TPE_max(TPE_Unit a, TPE_Unit b); +static inline TPE_Unit TPE_min(TPE_Unit a, TPE_Unit b); +static inline TPE_Unit TPE_nonZero(TPE_Unit x); +static inline TPE_Unit TPE_dist(TPE_Vec3 p1, TPE_Vec3 p2); +static inline TPE_Unit TPE_distApprox(TPE_Vec3 p1, TPE_Vec3 p2); +TPE_Unit TPE_sqrt(TPE_Unit x); + +/** Compute sine, TPE_FRACTIONS_PER_UNIT as argument corresponds to 2 * PI + radians. Returns a number from -TPE_FRACTIONS_PER_UNIT to + TPE_FRACTIONS_PER_UNIT. */ +TPE_Unit TPE_sin(TPE_Unit x); +TPE_Unit TPE_cos(TPE_Unit x); +TPE_Unit TPE_atan(TPE_Unit x); + +typedef struct +{ + TPE_Vec3 position; + TPE_UnitReduced velocity[3]; ///< not TPE_Vec3 to save size + uint8_t sizeDivided; /**< size (radius, ...), for saving space divided by + TPE_JOINT_SIZE_MULTIPLIER */ +} TPE_Joint; + +typedef struct +{ + uint8_t joint1; + uint8_t joint2; + uint16_t length; ///< connection's preferred length, uint16_t saves space +} TPE_Connection; + +#define TPE_BODY_FLAG_DEACTIVATED 1 /**< Not being updated due to low energy, + "sleeping", will be woken by + collisions etc. */ +#define TPE_BODY_FLAG_NONROTATING 2 /**< When set, the body won't rotate, + will only move linearly. Here the + velocity of the body's first joint + is the velocity of the whole + body. */ +#define TPE_BODY_FLAG_DISABLED 4 /**< Disabled, not taking part in + simulation. */ +#define TPE_BODY_FLAG_SOFT 8 /**< Soft connections, effort won't be + made to keep the body's shape. */ +#define TPE_BODY_FLAG_SIMPLE_CONN 16 /**< Simple connections, don't zero out + antagonist forces or apply + connection friction, can increase + performance. */ +#define TPE_BODY_FLAG_ALWAYS_ACTIVE 32 /**< Will never deactivate due to low + energy. */ + +/** Function used for defining static environment, working similarly to an SDF + (signed distance function). The parameters are: 3D point P, max distance D. + The function should behave like this: if P is inside the solid environment + volume, P will be returned; otherwise closest point (by Euclidean distance) to + the solid environment volume from P will be returned, except for a case when + this closest point would be further away than D, in which case any arbitrary + point further away than D may be returned (this allows for optimizations). */ +typedef TPE_Vec3 (*TPE_ClosestPointFunction)(TPE_Vec3, TPE_Unit); + +/** Function that can be used as a joint-joint or joint-environment collision + callback, parameters are following: body1 index, joint1 index, body2 index, + joint2 index, collision world position. If body1 index is the same as body1 + index, then collision type is body-environment, otherwise it is body-body + type. The function has to return either 1 if the collision is to be allowed + or 0 if it is to be discarded. This can besides others be used to disable + collisions between some bodies. */ +typedef uint8_t (*TPE_CollisionCallback)(uint16_t, uint16_t, uint16_t, uint16_t, + TPE_Vec3); + +/** Function used by the debug drawing functions to draw individual pixels to + the screen. The parameters are following: pixel x, pixel y, pixel color. */ +typedef void (*TPE_DebugDrawFunction)(uint16_t, uint16_t, uint8_t); + +/** Physics body made of spheres (each of same weight but possibly different + radia) connected by elastic springs. */ +typedef struct +{ + TPE_Joint *joints; + uint8_t jointCount; + TPE_Connection *connections; + uint8_t connectionCount; + TPE_UnitReduced jointMass; ///< mass of a single joint + TPE_UnitReduced friction; ///< friction of each joint + TPE_UnitReduced elasticity; ///< elasticity of each joint + uint8_t flags; + uint8_t deactivateCount; +} TPE_Body; + +typedef struct +{ + TPE_Body *bodies; + uint16_t bodyCount; + TPE_ClosestPointFunction environmentFunction; + TPE_CollisionCallback collisionCallback; +} TPE_World; + +/** Tests the mathematical validity of given closest point function (function + representing the physics environment), i.e. whether for example approaching + some closest point in a straight line keeps approximately the same closest + point. Note that this function may take a long time to complete, especially + with higher gridResolution values and more complex environment functions. You + should use this function to test your environment function, especially if you + create functions for your own shapes etc. The cornerFrom and cornerTo points + are corners of an axis-aligned box within which testing will take place, + gridResolution defines numbers of points (i.e. step length) along each + dimension to test (recommended e.g. 64), allowedError says error within which + points will be considered the same (recommended range approx. 10 to 200). If + testing is successful, 1 is returned, otherwise 0 is returned and the point + around which error was detected is returned in errorPoint (unless the pointer + is 0 in which case it is ignored). */ +uint8_t TPE_testClosestPointFunction(TPE_ClosestPointFunction f, + TPE_Vec3 cornerFrom, TPE_Vec3 cornerTo, uint8_t gridResolution, + TPE_UnitReduced allowedError, TPE_Vec3 *errorPoint); + +void TPE_bodyInit(TPE_Body *body, + TPE_Joint *joints, uint8_t jointCount, + TPE_Connection *connections, uint8_t connectionCount, + TPE_Unit mass); + +void TPE_worldInit(TPE_World *world, + TPE_Body *bodies, uint16_t bodyCount, + TPE_ClosestPointFunction environmentFunction); + +/** Gets orientation (rotation) of a body from a position of three of its + joints. The vector from joint1 to joint2 is considered the body's forward + direction, the vector from joint1 to joint3 its right direction. The returned + rotation is in Euler angles (see rotation conventions). */ +TPE_Vec3 TPE_bodyGetRotation(const TPE_Body *body, uint16_t joint1, + uint16_t joint2, uint16_t joint3); + +void TPE_vec3Normalize(TPE_Vec3 *v); + +/** Rotates a 3D point by given Euler angle rotation (see rotation + conventions). */ +TPE_Vec3 TPE_pointRotate(TPE_Vec3 point, TPE_Vec3 rotation); + +/** Returns an inverse rotation to given rotation, in Euler angles (see rotation + conventions). */ +TPE_Vec3 TPE_rotationInverse(TPE_Vec3 rotation); + +/** Returns a connection tension, i.e. a signed percentage difference against + desired length (TPE_FRACTIONS_PER_UNIT means 100%). */ +static inline TPE_Unit TPE_connectionTension(TPE_Unit length, + TPE_Unit desiredLength); + +/** Rotates a rotation specified in Euler angles by given axis + angle (see + rotation conventions). Returns a rotation in Eurler angles. */ +TPE_Vec3 TPE_rotationRotateByAxis(TPE_Vec3 rotation, TPE_Vec3 rotationByAxis); + +/** Computes the formula of a 1D collision of rigid bodies. */ +void TPE_getVelocitiesAfterCollision(TPE_Unit *v1, TPE_Unit *v2, TPE_Unit m1, + TPE_Unit m2, TPE_Unit elasticity); + +/** Computes orientation/rotation (see docs for orientation format) from two + vectors (which should be at least close to being perpendicular and do NOT + need to be normalized). This can be used to determine orientation of a body + from a relative position of its joints. */ +TPE_Vec3 TPE_rotationFromVecs(TPE_Vec3 forward, TPE_Vec3 right); + +TPE_Joint TPE_joint(TPE_Vec3 position, TPE_Unit size); + +/** Mostly for internal use, resolves a potential collision of two joints in a + way that keeps the joints outside provided environment (if the function + pointer is not 0). Returns 1 if joints collided or 0 otherwise. */ +uint8_t TPE_jointsResolveCollision(TPE_Joint *j1, TPE_Joint *j2, + TPE_Unit mass1, TPE_Unit mass2, TPE_Unit elasticity, TPE_Unit friction, + TPE_ClosestPointFunction env); + +/** Mostly for internal use, tests and potentially resolves a collision between + a joint and environment, returns 0 if no collision happened, 1 if it happened + and was resolved normally and 2 if it couldn't be resolved normally. */ +uint8_t TPE_jointEnvironmentResolveCollision(TPE_Joint *joint, TPE_Unit + elasticity, TPE_Unit friction, TPE_ClosestPointFunction env); + +/** Tests whether a body is currently colliding with the environment. */ +uint8_t TPE_bodyEnvironmentCollide(const TPE_Body *body, + TPE_ClosestPointFunction env); + +/** Mostly for internal use, tests and potentially resolves a collision of a + body with the environment, returns 1 if collision happened or 0 otherwise. */ +uint8_t TPE_bodyEnvironmentResolveCollision(TPE_Body *body, + TPE_ClosestPointFunction env); + +TPE_Vec3 TPE_bodyGetLinearVelocity(const TPE_Body *body); + +/** Computes the minimum bounding box of given body. */ +void TPE_bodyGetAABB(const TPE_Body *body, TPE_Vec3 *vMin, TPE_Vec3 *vMax); + +/** Computes a bounding sphere of a body which is not minimal but faster to + compute than the minimum bounding sphere. */ +void TPE_bodyGetFastBSphere(const TPE_Body *body, TPE_Vec3 *center, + TPE_Unit *radius); + +/** Computes the minimum bounding sphere of a body (there is another function + for a faster approximate bounding sphere). */ +void TPE_bodyGetBSphere(const TPE_Body *body, TPE_Vec3 *center, + TPE_Unit *radius); + +uint8_t TPE_checkOverlapAABB(TPE_Vec3 v1Min, TPE_Vec3 v1Max, TPE_Vec3 v2Min, + TPE_Vec3 v2Max); + +/** Mostly for internal use, checks and potentiall resolves collision of two + bodies so as to keep them outside given environment. Returns 1 if collision + happened or 0 otherwise. */ +uint8_t TPE_bodiesResolveCollision(TPE_Body *b1, TPE_Body *b2, + TPE_ClosestPointFunction env); + +/** Pins a joint of a body to specified location in space (sets its location + and zeros its velocity). */ +void TPE_jointPin(TPE_Joint *joint, TPE_Vec3 position); + +/** "Fakes" a rotation of a moving sphere by rotating it in the direction of + its movement; this can create the illusion of the sphere actually rotating + due to friction even if the physics sphere object (a body with a single joint) + isn't rotating at all. Returns a rotation in the "about axis" format (see + library conventions). */ +TPE_Vec3 TPE_fakeSphereRotation(TPE_Vec3 position1, TPE_Vec3 position2, + TPE_Unit radius); + +/** Casts a ray against environment and returns the closest hit of a surface. If + no surface was hit, a vector with all elements equal to TPE_INFINITY will be + returned. The function internally works differently for outside rays (rays + cast from the outside of the environment) and inside rays. Outside rays can + be traced with raymarching and will be processed very quickly and precisely; + in this case if any intersection is found, the function will try to return a + point outside (not guaranteed) the environment that's just in front of the hit + surface. Inside rays are difficult and slow to trace because environment + function won't provide distance, so the results aren't guaranteed to be + precise (the ray may miss some intersections); here rays will be traced by + given step (insideStepSize) and eventually iterated a bit towards the + intersection -- if any intersection is found, the function will try to return + a point inside (not guaranteed) the environment just before the hit + surface. */ +TPE_Vec3 TPE_castEnvironmentRay(TPE_Vec3 rayPos, TPE_Vec3 rayDir, + TPE_ClosestPointFunction environment, TPE_Unit insideStepSize, + TPE_Unit rayMarchMaxStep, uint32_t maxSteps); + +/** Casts a ray against bodies in a world (ignoring the environment), returns + the position of the closest hit as well as the hit body's index in bodyIndex + (unless the bodyIndex pointer is 0 in which case it is ignored). Similarly + with jointIndex. If no hit is found a vector with all elements equal to + TPE_INFINITY will be returned and bodyIndex will be -1. A specific body can be + excluded with excludeBody (negative value will just make this parameter + ignored). */ +TPE_Vec3 TPE_castBodyRay(TPE_Vec3 rayPos, TPE_Vec3 rayDir, int16_t excludeBody, + const TPE_World *world, int16_t *bodyIndex, int16_t *jointIndex); + +/** Performs one step (tick, frame, ...) of the physics world simulation + including updating positions and velocities of bodies, collision detection and + resolution, possible reshaping or deactivation of inactive bodies etc. The + time length of the step is relative to all other units but it's ideal if it is + 1/30th of a second. */ +void TPE_worldStep(TPE_World *world); + +void TPE_worldDeactivateAll(TPE_World *world); +void TPE_worldActivateAll(TPE_World *world); + +TPE_Unit TPE_worldGetNetSpeed(const TPE_World *world); +TPE_Unit TPE_bodyGetNetSpeed(const TPE_Body *body); +TPE_Unit TPE_bodyGetAverageSpeed(const TPE_Body *body); +void TPE_bodyMultiplyNetSpeed(TPE_Body *body, TPE_Unit factor); +void TPE_bodyLimitAverageSpeed(TPE_Body *body, TPE_Unit speedMin, + TPE_Unit speedMax); + +/** Deactivates a body (puts it to sleep until another collision or force wake + up). */ +void TPE_bodyDeactivate(TPE_Body *body); + +static inline uint8_t TPE_bodyIsActive(const TPE_Body *body); + +/** Attempts to shift the joints of a soft body so that the tension of all + springs becomes zero while keeping the joints near their current position. + This function performs one iteration of the equalizing algorithm and doesn't + guarantee a perfect solution, it may help to run multiple iterations (call + this function multiple times). */ +void TPE_bodyReshape(TPE_Body *body, TPE_ClosestPointFunction + environmentFunction); + +/** Mostly for internal use, performs some "magic" on body connections, mainly + cancelling out of velocities going against each other and also applying + connection friction in soft bodies. The strong parameter indicates if the + body is soft or not. */ +void TPE_bodyCancelOutVelocities(TPE_Body *body, uint8_t strong); + +/** Moves a body by certain offset. */ +void TPE_bodyMoveBy(TPE_Body *body, TPE_Vec3 offset); + +/** Moves a body (its center of mass) to given position. */ +void TPE_bodyMoveTo(TPE_Body *body, TPE_Vec3 position); + +/** Zeros velocities of all soft body joints. */ +void TPE_bodyStop(TPE_Body *body); + +void TPE_bodyActivate(TPE_Body *body); + +/** Adds velocity to a soft body. */ +void TPE_bodyAccelerate(TPE_Body *body, TPE_Vec3 velocity); + +void TPE_bodyApplyGravity(TPE_Body *body, TPE_Unit downwardsAccel); + +/** Adds angular velocity to a soft body. The rotation vector specifies the axis + of rotation by its direction and angular velocity by its magnitude (magnitude + of TPE_FRACTIONS_PER_UNIT will add linear velocity of TPE_FRACTIONS_PER_UNIT + per tick to a point in the distance of TPE_FRACTIONS_PER_UNIT from the + rotation axis). */ +void TPE_bodySpin(TPE_Body *body, TPE_Vec3 rotation); + +/** Same as TPE_bodySpin but additionally allows to specify the center of + the spin. */ +void TPE_bodySpinWithCenter(TPE_Body *body, TPE_Vec3 rotation, TPE_Vec3 center); + +/** Instantly rotates a body about an axis (see library conventions for + the rotation format). */ +void TPE_bodyRotateByAxis(TPE_Body *body, TPE_Vec3 rotation); + +/** Computes the center of mass of a body. This averages the position of all + joints; note that if you need, you may estimate the center of the body faster, + e.g. by taking a position of a single "center joint", or averaging just 2 + extreme points. */ +TPE_Vec3 TPE_bodyGetCenterOfMass(const TPE_Body *body); + +/** Draws a debug view of a 3D physics world using a provided pixel drawing + function. This can be used to overlay a simple visualization of the physics + objects to your main render, to spot exact borders of objects etc. The + function draws simple dotted lines and circles with different "colors" for + different types of objects (joints, connections, environemnt). camPos, camRot + and camView should match the camera settings of your main renderer. CamView.x + is horizontal resolution in pixels, camView.y is the vertical resolution, + CamView.z says the camera focal length (~FOV) in TPE_Units (0 means + orthographic projection). envGridRes is the resolution of an environment probe + grid (the function will probe points in space and draw borders of the physics + environemnt), envGridSize is the size (int TPE_Units) of the grid cell. Note + the function may be slow (reducing envGridRes can help, workable value can be + e.g. 16). */ +void TPE_worldDebugDraw(TPE_World *world, TPE_DebugDrawFunction drawFunc, + TPE_Vec3 camPos, TPE_Vec3 camRot, TPE_Vec3 camView, uint16_t envGridRes, + TPE_Unit envGridSize); + +#define TPE_DEBUG_COLOR_CONNECTION 0 +#define TPE_DEBUG_COLOR_JOINT 1 +#define TPE_DEBUG_COLOR_ENVIRONMENT 2 +#define TPE_DEBUG_COLOR_INACTIVE 3 + +uint32_t TPE_jointHash(const TPE_Joint *joint); +uint32_t TPE_connectionHash(const TPE_Connection *connection); +uint32_t TPE_bodyHash(const TPE_Body *body); + +/** Computes 32 bit hash of the world, useful for checking if two states of the + world differ. The function takes into account most of the relevant state but + possibly not all of it, for details check the code. */ +uint32_t TPE_worldHash(const TPE_World *world); + +// FUNCTIONS FOR GENERATING BODIES + +void TPE_makeBox(TPE_Joint joints[8], TPE_Connection connections[16], + TPE_Unit width, TPE_Unit depth, TPE_Unit height, TPE_Unit jointSize); +void TPE_makeCenterBox(TPE_Joint joints[9], TPE_Connection connections[18], + TPE_Unit width, TPE_Unit depth, TPE_Unit height, TPE_Unit jointSize); +void TPE_makeRect(TPE_Joint joints[4], TPE_Connection connections[6], + TPE_Unit width, TPE_Unit depth, TPE_Unit jointSize); +void TPE_makeTriangle(TPE_Joint joints[3], TPE_Connection connections[3], + TPE_Unit sideLength, TPE_Unit jointSize); +void TPE_makeCenterRect(TPE_Joint joints[5], TPE_Connection connections[8], + TPE_Unit width, TPE_Unit depth, TPE_Unit jointSize); +void TPE_makeCenterRectFull(TPE_Joint joints[5], TPE_Connection connections[10], + TPE_Unit width, TPE_Unit depth, TPE_Unit jointSize); +void TPE_make2Line(TPE_Joint joints[2], TPE_Connection connections[1], + TPE_Unit length, TPE_Unit jointSize); + +// FUNCTIONS FOR BUILDING ENVIRONMENT + +TPE_Vec3 TPE_envAABoxInside(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 size); +TPE_Vec3 TPE_envAABox(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 maxCornerVec); +TPE_Vec3 TPE_envBox(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 maxCornerVec, + TPE_Vec3 rotation); +TPE_Vec3 TPE_envSphere(TPE_Vec3 point, TPE_Vec3 center, TPE_Unit radius); +TPE_Vec3 TPE_envSphereInside(TPE_Vec3 point, TPE_Vec3 center, TPE_Unit radius); +TPE_Vec3 TPE_envHalfPlane(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 normal); +TPE_Vec3 TPE_envGround(TPE_Vec3 point, TPE_Unit height); +TPE_Vec3 TPE_envInfiniteCylinder(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 + direction, TPE_Unit radius); +TPE_Vec3 TPE_envCylinder(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 direction, + TPE_Unit radius); +TPE_Vec3 TPE_envCone(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 direction, + TPE_Unit radius); +TPE_Vec3 TPE_envLineSegment(TPE_Vec3 point, TPE_Vec3 a, TPE_Vec3 b); +TPE_Vec3 TPE_envHeightmap(TPE_Vec3 point, TPE_Vec3 center, TPE_Unit gridSize, + TPE_Unit (*heightFunction)(int32_t x, int32_t y), TPE_Unit maxDist); + +/** Environment function for triagnular prism, e.g. for ramps. The sides array + contains three 2D coordinates of points of the triangle in given plane with + respect to the center. WARNING: the points must be specified in counter + clowckwise direction! The direction var specified axis direction (0, 1 or + 2).*/ +TPE_Vec3 TPE_envAATriPrism(TPE_Vec3 point, TPE_Vec3 center, + const TPE_Unit sides[6], TPE_Unit depth, uint8_t direction); + +/* The following are helper macros for creating a union of shapes inside an + environment function and accelerating them with bounding volumes. */ + +#define TPE_ENV_START(test,point) TPE_Vec3 _pBest = test, _pTest; \ + TPE_Unit _dBest = TPE_DISTANCE(_pBest,point), _dTest; \ + (void)(_pBest); (void)(_dBest); (void)(_dTest); (void)(_pTest); // supress war + +#define TPE_ENV_NEXT(test,point) \ + { if (_pBest.x == point.x && _pBest.y == point.y && _pBest.z == point.z) \ + return _pBest; \ + _pTest = test; _dTest = TPE_DISTANCE(_pTest,point); \ + if (_dTest < _dBest) { _pBest = _pTest; _dBest = _dTest; } } + +#define TPE_ENV_END return _pBest; + +#define TPE_ENV_BCUBE_TEST(bodyBCubeC,bodyBCubeR,envBCubeC,envBCubeR) ( \ + (TPE_abs(envBCubeC.x - bodyBCubeC.x) <= ((bodyBCubeR) + (envBCubeR))) && \ + (TPE_abs(envBCubeC.y - bodyBCubeC.y) <= ((bodyBCubeR) + (envBCubeR))) && \ + (TPE_abs(envBCubeC.z - bodyBCubeC.z) <= ((bodyBCubeR) + (envBCubeR)))) + +#define TPE_ENV_BSPHERE_TEST(bodyBSphereC,bodyBSphereR,envBSphereC,envBSphereR)\ + (TPE_DISTANCE(bodyBSphereC,envBSphereC) <= ((bodyBSphereR) + (envBSphereR))) + +//------------------------------------------------------------------------------ +// privates: + +uint16_t _TPE_body1Index, _TPE_body2Index, _TPE_joint1Index, _TPE_joint2Index; +TPE_CollisionCallback _TPE_collisionCallback; + +static inline TPE_Unit TPE_nonZero(TPE_Unit x) +{ + return x != 0 ? x : 1; +} + +static inline TPE_Unit TPE_connectionTension(TPE_Unit length, + TPE_Unit desiredLength) +{ + return (length * TPE_F) / desiredLength + - TPE_F; +} + +TPE_Joint TPE_joint(TPE_Vec3 position, TPE_Unit size) +{ + TPE_Joint result; + + result.velocity[0] = 0; + result.velocity[1] = 0; + result.velocity[2] = 0; + + result.position = position; + + size /= TPE_JOINT_SIZE_MULTIPLIER; + + if (size > 0xff) + { + TPE_LOG("WARNING: joint size too big in TPE_joint"); + } + + result.sizeDivided = size; + + return result; +} + +TPE_Vec3 TPE_vec3(TPE_Unit x, TPE_Unit y, TPE_Unit z) +{ + TPE_Vec3 r; + + r.x = x; + r.y = y; + r.z = z; + + return r; +} + +TPE_Unit TPE_sqrt(TPE_Unit x) +{ + int8_t sign = 1; + + if (x < 0) + { + sign = -1; + x *= -1; + } + + uint32_t result = 0; + uint32_t a = x; + uint32_t b = 1u << 30; + + while (b > a) + b >>= 2; + + while (b != 0) + { + if (a >= result + b) + { + a -= result + b; + result = result + 2 * b; + } + + b >>= 2; + result >>= 1; + } + + return result * sign; +} + +TPE_Unit TPE_vec3Len(TPE_Vec3 v) +{ +#define ANTI_OVERFLOW 25000 + if (v.x < ANTI_OVERFLOW && v.x > -1 * ANTI_OVERFLOW && + v.y < ANTI_OVERFLOW && v.y > -1 * ANTI_OVERFLOW && + v.z < ANTI_OVERFLOW && v.z > -1 * ANTI_OVERFLOW) + { + return TPE_sqrt(v.x * v.x + v.y * v.y + v.z * v.z); + } + else + { + v.x /= 32; v.y /= 32; v.z /= 32; + return TPE_sqrt(v.x * v.x + v.y * v.y + v.z * v.z) * 32; + } +#undef ANTI_OVERFLOW +} + +TPE_Unit TPE_vec3LenApprox(TPE_Vec3 v) +{ + // 48 sided polyhedron approximation + + if (v.x < 0) v.x *= -1; + if (v.y < 0) v.y *= -1; + if (v.z < 0) v.z *= -1; + + if (v.x < v.y) // order the coordinates + { + if (v.x < v.z) + { + if (v.y < v.z) + { // v.x < v.y < v.z + int32_t t = v.x; v.x = v.z; v.z = t; + } + else + { // v.x < v.z < v.y + int32_t t = v.x; v.x = v.y; v.y = t; + t = v.z; v.z = v.y; v.y = t; + } + } + else + { // v.z < v.x < v.y + int32_t t = v.x; v.x = v.y; v.y = t; + } + } + else + { + if (v.y < v.z) + { + if (v.x < v.z) + { // v.y < v.x < v.z + int32_t t = v.y; v.y = v.z; v.z = t; + t = v.x; v.x = v.y; v.y = t; + } + else + { // v.y < v.z < v.x + int32_t t = v.y; v.y = v.z; v.z = t; + } + } + } + + return (893 * v.x + 446 * v.y + 223 * v.z) / 1024; +} + +TPE_Unit TPE_dist(TPE_Vec3 p1, TPE_Vec3 p2) +{ + p1 = TPE_vec3Minus(p1,p2); + return TPE_vec3Len(p1); +} + +TPE_Unit TPE_distApprox(TPE_Vec3 p1, TPE_Vec3 p2) +{ + p1 = TPE_vec3Minus(p1,p2); + return TPE_vec3LenApprox(p1); +} + +void TPE_bodyInit(TPE_Body *body, + TPE_Joint *joints, uint8_t jointCount, + TPE_Connection *connections, uint8_t connectionCount, + TPE_Unit mass) +{ + body->joints = joints; + body->jointCount = jointCount; + body->connections = connections; + body->connectionCount = connectionCount; + body->deactivateCount = 0; + body->friction = TPE_F / 2; + body->elasticity = TPE_F / 2; + body->flags = 0; + body->jointMass = TPE_nonZero(mass / jointCount); + + for (uint32_t i = 0; i < connectionCount; ++i) + { + TPE_Unit d = TPE_DISTANCE( + joints[connections[i].joint1].position, + joints[connections[i].joint2].position); + + if (d > 0xffff) + { + TPE_LOG("WARNING: joint distance too long in TPE_bodyInit"); + } + + connections[i].length = d != 0 ? d : 1; // prevent later division by zero + } +} + +void TPE_worldInit(TPE_World *world, TPE_Body *bodies, uint16_t bodyCount, + TPE_ClosestPointFunction environmentFunction) +{ + world->bodies = bodies; + world->bodyCount = bodyCount; + world->environmentFunction = environmentFunction; + world->collisionCallback = 0; +} + +#define C(n,a,b) connections[n].joint1 = a; connections[n].joint2 = b; + +void TPE_make2Line(TPE_Joint joints[2], TPE_Connection connections[1], + TPE_Unit length, TPE_Unit jointSize) +{ + joints[0] = TPE_joint(TPE_vec3(length / 2,0,0),jointSize); + joints[1] = TPE_joint(TPE_vec3(length / -2,0,0),jointSize); + C(0, 0,1) +} + +void TPE_makeRect(TPE_Joint joints[4], TPE_Connection connections[6], + TPE_Unit width, TPE_Unit depth, TPE_Unit jointSize) +{ + width /= 2; + depth /= 2; + + for (uint8_t i = 0; i < 4; ++i) + joints[i] = TPE_joint(TPE_vec3((i % 2) ? -1 * width : width, + 0,(i / 2) ? - 1 * depth : depth),jointSize); + + C(0, 0,1) C(1, 0,2) C (2, 3,1) C(3, 3,2) + C(4, 0,3) C(5, 1,2) +} + +void TPE_makeCenterRect(TPE_Joint joints[5], TPE_Connection connections[8], + TPE_Unit width, TPE_Unit depth, TPE_Unit jointSize) +{ + TPE_makeRect(joints,connections,width,depth,jointSize); + + joints[4] = TPE_joint(TPE_vec3(0,0,0),jointSize); + + C(6, 0,4) C(7, 3,4) +} + +void TPE_makeCenterRectFull(TPE_Joint joints[5], TPE_Connection connections[10], + TPE_Unit width, TPE_Unit depth, TPE_Unit jointSize) +{ + TPE_makeCenterRect(joints,connections,width,depth,jointSize); + C(8, 1,4) C(9, 2,4) +} + +void TPE_makeTriangle(TPE_Joint joints[3], TPE_Connection connections[3], + TPE_Unit sideLength, TPE_Unit jointSize) +{ + joints[0] = TPE_joint(TPE_vec3(sideLength / 2,0, + TPE_sqrt((sideLength * sideLength) / 2) / 2),jointSize); + + joints[1] = joints[0]; + joints[1].position.x *= -1; + joints[2] = TPE_joint(TPE_vec3(0,0,-1 * joints[0].position.z),jointSize); + + C(0, 0,1) C(1, 1,2) C(2, 2,0) +} + +void TPE_makeBox(TPE_Joint joints[8], TPE_Connection connections[16], + TPE_Unit width, TPE_Unit depth, TPE_Unit height, TPE_Unit jointSize) +{ + width /= 2; + depth /= 2; + height /= 2; + + for (uint8_t i = 0; i < 8; ++i) + joints[i] = TPE_joint( + TPE_vec3( + (i % 2) ? width : (-1 * width), + ((i >> 2) % 2) ? height : (-1 * height), + ((i >> 1) % 2) ? depth : (-1 * depth)), + jointSize); + + C(0, 0,1) C(1, 1,3) C(2, 3,2) C(3, 2,0) // top + C(4, 4,5) C(5, 5,7) C(6, 7,6) C(7, 6,4) // bottom + C(8, 0,4) C(9, 1,5) C(10,3,7) C(11,2,6) // middle + C(12,0,7) C(13,1,6) C(14,2,5) C(15,3,4) // diagonal +} + +void TPE_makeCenterBox(TPE_Joint joints[9], TPE_Connection connections[18], + TPE_Unit width, TPE_Unit depth, TPE_Unit height, TPE_Unit jointSize) +{ + TPE_makeBox(joints,connections,width,depth,height,jointSize); + + joints[8] = TPE_joint(TPE_vec3(0,0,0),jointSize); + + C(16, 0,8) C(17, 7,8) +} + +#undef C + +void TPE_bodyDeactivate(TPE_Body *body) +{ + body->flags |= TPE_BODY_FLAG_DEACTIVATED; +} + +void TPE_worldStep(TPE_World *world) +{ + _TPE_collisionCallback = world->collisionCallback; + + for (uint16_t i = 0; i < world->bodyCount; ++i) + { + TPE_Body *body = world->bodies + i; + + if (body->flags & (TPE_BODY_FLAG_DEACTIVATED | TPE_BODY_FLAG_DISABLED)) + continue; + + TPE_Joint *joint = body->joints, *joint2; + + TPE_Vec3 origPos = body->joints[0].position; + + for (uint16_t j = 0; j < body->jointCount; ++j) // apply velocities + { + // non-rotating bodies will copy the 1st joint's velocity + + if (body->flags & TPE_BODY_FLAG_NONROTATING) + for (uint8_t k = 0; k < 3; ++k) + joint->velocity[k] = body->joints[0].velocity[k]; + + joint->position.x += joint->velocity[0]; + joint->position.y += joint->velocity[1]; + joint->position.z += joint->velocity[2]; + + joint++; + } + + TPE_Connection *connection = body->connections; + + TPE_Vec3 aabbMin, aabbMax; + + TPE_bodyGetAABB(body,&aabbMin,&aabbMax); + + _TPE_body1Index = i; + + _TPE_body2Index = _TPE_body1Index; + + uint8_t collided = + TPE_bodyEnvironmentResolveCollision(body,world->environmentFunction); + + if (body->flags & TPE_BODY_FLAG_NONROTATING) + { + /* Non-rotating bodies may end up still colliding after environment coll + resolvement (unlike rotating bodies where each joint is ensured separately + to not collide). So if still in collision, we try a few more times. If not + successful, we simply undo any shifts we've done. This should absolutely + prevent any body escaping out of environment bounds. */ + + for (uint8_t i = 0; i < TPE_NONROTATING_COLLISION_RESOLVE_ATTEMPTS; ++i) + { + if (!collided) + break; + + collided = + TPE_bodyEnvironmentResolveCollision(body,world->environmentFunction); + } + + if (collided && + TPE_bodyEnvironmentCollide(body,world->environmentFunction)) + TPE_bodyMoveBy(body,TPE_vec3Minus(origPos,body->joints[0].position)); + } + else // normal, rotating bodies + { + TPE_Unit bodyTension = 0; + + for (uint16_t j = 0; j < body->connectionCount; ++j) // joint tension + { + joint = &(body->joints[connection->joint1]); + joint2 = &(body->joints[connection->joint2]); + + TPE_Vec3 dir = TPE_vec3Minus(joint2->position,joint->position); + + TPE_Unit tension = TPE_connectionTension(TPE_LENGTH(dir), + connection->length); + + bodyTension += tension > 0 ? tension : -tension; + + if (tension > TPE_TENSION_ACCELERATION_THRESHOLD || + tension < -1 * TPE_TENSION_ACCELERATION_THRESHOLD) + { + TPE_vec3Normalize(&dir); + + if (tension > TPE_TENSION_GREATER_ACCELERATION_THRESHOLD || + tension < -1 * TPE_TENSION_GREATER_ACCELERATION_THRESHOLD) + { + /* apply twice the acceleration after a second threshold, not so + elegant but seems to work :) */ + dir.x *= 2; + dir.y *= 2; + dir.z *= 2; + } + + dir.x /= TPE_TENSION_ACCELERATION_DIVIDER; + dir.y /= TPE_TENSION_ACCELERATION_DIVIDER; + dir.z /= TPE_TENSION_ACCELERATION_DIVIDER; + + if (tension < 0) + { + dir.x *= -1; + dir.y *= -1; + dir.z *= -1; + } + + joint->velocity[0] += dir.x; + joint->velocity[1] += dir.y; + joint->velocity[2] += dir.z; + + joint2->velocity[0] -= dir.x; + joint2->velocity[1] -= dir.y; + joint2->velocity[2] -= dir.z; + } + + connection++; + } + + if (body->connectionCount > 0) + { + uint8_t hard = !(body->flags & TPE_BODY_FLAG_SOFT); + + if (hard) + { + TPE_bodyReshape(body,world->environmentFunction); + + bodyTension /= body->connectionCount; + + if (bodyTension > TPE_RESHAPE_TENSION_LIMIT) + for (uint8_t k = 0; k < TPE_RESHAPE_ITERATIONS; ++k) + TPE_bodyReshape(body,world->environmentFunction); + } + + if (!(body->flags & TPE_BODY_FLAG_SIMPLE_CONN)) + TPE_bodyCancelOutVelocities(body,hard); + } + } + + for (uint16_t j = 0; j < world->bodyCount; ++j) + { + if (j > i || (world->bodies[j].flags & TPE_BODY_FLAG_DEACTIVATED)) + { + // firstly quick-check collision of body AA bounding boxes + + TPE_Vec3 aabbMin2, aabbMax2; + TPE_bodyGetAABB(&world->bodies[j],&aabbMin2,&aabbMax2); + + _TPE_body2Index = j; + + if (TPE_checkOverlapAABB(aabbMin,aabbMax,aabbMin2,aabbMax2) && + TPE_bodiesResolveCollision(body,world->bodies + j, + world->environmentFunction)) + { + TPE_bodyActivate(body); + body->deactivateCount = TPE_LIGHT_DEACTIVATION; + + TPE_bodyActivate(world->bodies + j); + world->bodies[j].deactivateCount = TPE_LIGHT_DEACTIVATION; + } + } + } + + if (!(body->flags & TPE_BODY_FLAG_ALWAYS_ACTIVE)) + { + if (body->deactivateCount >= TPE_DEACTIVATE_AFTER) + { + TPE_bodyStop(body); + body->deactivateCount = 0; + body->flags |= TPE_BODY_FLAG_DEACTIVATED; + } + else if (TPE_bodyGetAverageSpeed(body) <= TPE_LOW_SPEED) + body->deactivateCount++; + else + body->deactivateCount = 0; + } + } +} + +void TPE_bodyActivate(TPE_Body *body) +{ + // the if check has to be here, don't remove it + + if (body->flags & TPE_BODY_FLAG_DEACTIVATED) + { + TPE_bodyStop(body); + body->flags &= ~TPE_BODY_FLAG_DEACTIVATED; + body->deactivateCount = 0; + } +} + +TPE_Unit TPE_bodyGetNetSpeed(const TPE_Body *body) +{ +#if TPE_APPROXIMATE_NET_SPEED + TPE_Vec3 netV = TPE_vec3(0,0,0); + + const TPE_Joint *joint = body->joints; + + for (uint16_t i = 0; i < body->jointCount; ++i) + { + netV.x += TPE_abs(joint->velocity[0]); + netV.y += TPE_abs(joint->velocity[1]); + netV.z += TPE_abs(joint->velocity[2]); + + joint++; + } + + return TPE_vec3LenApprox(netV); +#else + TPE_Unit velocity = 0; + + const TPE_Joint *joint = body->joints; + + for (uint16_t i = 0; i < body->jointCount; ++i) + { + velocity += TPE_LENGTH( + TPE_vec3(joint->velocity[0],joint->velocity[1],joint->velocity[2])); + + joint++; + } + + return velocity; +#endif +} + +TPE_Unit TPE_bodyGetAverageSpeed(const TPE_Body *body) +{ + return TPE_bodyGetNetSpeed(body) / body->jointCount; +} + +void TPE_bodyMultiplyNetSpeed(TPE_Body *body, TPE_Unit factor) +{ + TPE_Joint *joint = body->joints; + + for (uint16_t j = 0; j < body->jointCount; ++j) + { + for (uint8_t k = 0; k < 3; ++k) + joint->velocity[k] = + (((TPE_Unit) joint->velocity[k]) * factor) / + TPE_F; + + joint++; + } +} + +void TPE_bodyLimitAverageSpeed(TPE_Body *body, TPE_Unit speedMin, + TPE_Unit speedMax) +{ + for (uint8_t i = 0; i < 16; ++i) + { + TPE_Unit speed = TPE_bodyGetAverageSpeed(body); + + if (speed >= speedMin && speed <= speedMax) + return; + + TPE_Unit fraction = + (((speedMax + speedMin) / 2) * TPE_F) / + TPE_nonZero(speed); + + TPE_bodyMultiplyNetSpeed(body,fraction); + } +} + +void TPE_bodyCancelOutVelocities(TPE_Body *body, uint8_t strong) +{ + for (uint16_t i = 0; i < body->connectionCount; ++i) + { + TPE_Connection *c = &body->connections[i]; + + TPE_Joint *j1 = &(body->joints[c->joint1]); + TPE_Joint *j2 = &(body->joints[c->joint2]); + + TPE_Vec3 dir = TPE_vec3Minus(j2->position,j1->position); + + TPE_Unit len = TPE_nonZero(TPE_LENGTH(dir)); + + uint8_t cancel = 1; + + if (strong) + { + TPE_Unit tension = TPE_connectionTension(len,c->length); + + cancel = tension <= TPE_TENSION_ACCELERATION_THRESHOLD && + tension >= -1 * TPE_TENSION_ACCELERATION_THRESHOLD; + } + + if (cancel) + { + TPE_Vec3 + v1 = TPE_vec3(j1->velocity[0],j1->velocity[1],j1->velocity[2]), + v2 = TPE_vec3(j2->velocity[0],j2->velocity[1],j2->velocity[2]); + + dir.x = (dir.x * TPE_F) / len; // normalize + dir.y = (dir.y * TPE_F) / len; + dir.z = (dir.z * TPE_F) / len; + + v1 = TPE_vec3ProjectNormalized(v1,dir); + v2 = TPE_vec3ProjectNormalized(v2,dir); + + TPE_Vec3 avg = TPE_vec3Plus(v1,v2); + + avg.x /= 2; + avg.y /= 2; + avg.z /= 2; + + if (strong) + { + j1->velocity[0] = j1->velocity[0] - v1.x + avg.x; + j1->velocity[1] = j1->velocity[1] - v1.y + avg.y; + j1->velocity[2] = j1->velocity[2] - v1.z + avg.z; + + j2->velocity[0] = j2->velocity[0] - v2.x + avg.x; + j2->velocity[1] = j2->velocity[1] - v2.y + avg.y; + j2->velocity[2] = j2->velocity[2] - v2.z + avg.z; + } + else + { + j1->velocity[0] = j1->velocity[0] - v1.x + (v1.x * 3 + avg.x) / 4; + j1->velocity[1] = j1->velocity[1] - v1.y + (v1.y * 3 + avg.y) / 4; + j1->velocity[2] = j1->velocity[2] - v1.z + (v1.z * 3 + avg.z) / 4; + + j2->velocity[0] = j2->velocity[0] - v2.x + (v2.x * 3 + avg.x) / 4; + j2->velocity[1] = j2->velocity[1] - v2.y + (v2.y * 3 + avg.y) / 4; + j2->velocity[2] = j2->velocity[2] - v2.z + (v2.z * 3 + avg.z) / 4; + } + } + } +} + +void TPE_bodyReshape(TPE_Body *body, + TPE_ClosestPointFunction environmentFunction) +{ + for (uint16_t i = 0; i < body->connectionCount; ++i) + { + TPE_Connection *c = &body->connections[i]; + + TPE_Joint *j1 = &(body->joints[c->joint1]); + TPE_Joint *j2 = &(body->joints[c->joint2]); + + TPE_Vec3 dir = TPE_vec3Minus(j2->position,j1->position); + + TPE_Vec3 middle = TPE_vec3Plus(j1->position,j2->position); + + middle.x /= 2; + middle.y /= 2; + middle.z /= 2; + + TPE_vec3Normalize(&dir); + + dir.x = (dir.x * c->length) / TPE_F; + dir.y = (dir.y * c->length) / TPE_F; + dir.z = (dir.z * c->length) / TPE_F; + + TPE_Vec3 positionBackup = j1->position; + + j1->position.x = middle.x - dir.x / 2; + j1->position.y = middle.y - dir.y / 2; + j1->position.z = middle.z - dir.z / 2; + + if (environmentFunction != 0 && TPE_LENGTH(TPE_vec3Minus(j1->position, + environmentFunction(j1->position,TPE_JOINT_SIZE(*j1)))) + < TPE_JOINT_SIZE(*j1)) + j1->position = positionBackup; + + positionBackup = j2->position; + + j2->position.x = j1->position.x + dir.x; + j2->position.y = j1->position.y + dir.y; + j2->position.z = j1->position.z + dir.z; + + if (environmentFunction != 0 && TPE_LENGTH(TPE_vec3Minus(j2->position, + environmentFunction(j2->position,TPE_JOINT_SIZE(*j2)))) + < TPE_JOINT_SIZE(*j2)) + j2->position = positionBackup; + } +} + +TPE_Vec3 TPE_vec3Plus(TPE_Vec3 v1, TPE_Vec3 v2) +{ + v1.x += v2.x; + v1.y += v2.y; + v1.z += v2.z; + + return v1; +} + +TPE_Vec3 TPE_vec3Minus(TPE_Vec3 v1, TPE_Vec3 v2) +{ + v1.x -= v2.x; + v1.y -= v2.y; + v1.z -= v2.z; + + return v1; +} + +void TPE_vec3Normalize(TPE_Vec3 *v) +{ + TPE_Unit l = TPE_LENGTH(*v); + + if (l == 0) + *v = TPE_vec3(TPE_F,0,0); + else + { + if (l < 16) // too short vec would cause inacurracte normalization + { + v->x *= 8; + v->y *= 8; + v->z *= 8; + l = TPE_LENGTH(*v); + } + + v->x = (v->x * TPE_F) / l; + v->y = (v->y * TPE_F) / l; + v->z = (v->z * TPE_F) / l; + } +} + +TPE_Vec3 TPE_bodyGetRotation(const TPE_Body *body, uint16_t joint1, + uint16_t joint2, uint16_t joint3) +{ + return TPE_rotationFromVecs( + TPE_vec3Minus( + body->joints[joint2].position, + body->joints[joint1].position), + TPE_vec3Minus( + body->joints[joint3].position, + body->joints[joint1].position)); +} + +TPE_Vec3 TPE_bodyGetCenterOfMass(const TPE_Body *body) +{ + // note that joint sizes don't play a role as all weight the same + + TPE_Vec3 result = TPE_vec3(0,0,0); + + const TPE_Joint *j = body->joints; + + for (uint16_t i = 0; i < body->jointCount; ++i) + { + result = TPE_vec3Plus(result,j->position); + j++; + } + + result.x /= body->jointCount; + result.y /= body->jointCount; + result.z /= body->jointCount; + + return result; +} + +void TPE_bodySpinWithCenter(TPE_Body *body, TPE_Vec3 rotation, TPE_Vec3 center) +{ + for (uint16_t i = 0; i < body->jointCount; ++i) + { + TPE_Joint *j = body->joints + i; + + TPE_Vec3 toPoint = TPE_vec3Minus(j->position,center); + + toPoint = TPE_vec3Project(toPoint,rotation); + toPoint = TPE_vec3Plus(center,toPoint); + toPoint = TPE_vec3Minus(j->position,toPoint); + toPoint = TPE_vec3Cross(toPoint,rotation); + + j->velocity[0] += toPoint.x; + j->velocity[1] += toPoint.y; + j->velocity[2] += toPoint.z; + } +} + +void TPE_bodySpin(TPE_Body *body, TPE_Vec3 rotation) +{ + TPE_bodySpinWithCenter(body,rotation,TPE_bodyGetCenterOfMass(body)); +} + +TPE_Vec3 _TPE_rotateByAxis(TPE_Vec3 p, TPE_Vec3 axisNormalized, TPE_Unit angle) +{ + TPE_Vec3 projected = TPE_vec3ProjectNormalized(p,axisNormalized); + + TPE_Vec3 a = TPE_vec3Minus(p,projected); + + if (a.x == 0 && a.y == 0 && a.z == 0) + return p; + + TPE_Vec3 b = TPE_vec3Cross(a,axisNormalized); + + return TPE_vec3Plus(projected,TPE_vec3Plus( + TPE_vec3Times(a,TPE_cos(angle)), + TPE_vec3Times(b,TPE_sin(angle)))); +} + +void TPE_bodyRotateByAxis(TPE_Body *body, TPE_Vec3 rotation) +{ + TPE_Vec3 bodyCenter = TPE_bodyGetCenterOfMass(body); + TPE_Unit angle = TPE_LENGTH(rotation); + + TPE_vec3Normalize(&rotation); + + for (uint16_t i = 0; i < body->jointCount; ++i) + { + TPE_Vec3 toPoint = TPE_vec3Minus(body->joints[i].position,bodyCenter); + body->joints[i].position = TPE_vec3Plus(bodyCenter, + _TPE_rotateByAxis(toPoint,rotation,angle)); + } +} + +TPE_Vec3 TPE_vec3Cross(TPE_Vec3 v1, TPE_Vec3 v2) +{ + TPE_Vec3 r; + + r.x = (v1.y * v2.z - v1.z * v2.y) / TPE_F; + r.y = (v1.z * v2.x - v1.x * v2.z) / TPE_F; + r.z = (v1.x * v2.y - v1.y * v2.x) / TPE_F; + + return r; +} + +TPE_Vec3 TPE_vec3ProjectNormalized(TPE_Vec3 v, TPE_Vec3 baseNormalized) +{ + TPE_Vec3 r; + + TPE_Unit p = TPE_vec3Dot(v,baseNormalized); + + r.x = (p * baseNormalized.x) / TPE_F; + r.y = (p * baseNormalized.y) / TPE_F; + r.z = (p * baseNormalized.z) / TPE_F; + + return r; +} + +TPE_Vec3 TPE_vec3Project(TPE_Vec3 v, TPE_Vec3 base) +{ + TPE_vec3Normalize(&base); + + return TPE_vec3ProjectNormalized(v,base); +} + +void TPE_bodyMoveBy(TPE_Body *body, TPE_Vec3 offset) +{ + for (uint16_t i = 0; i < body->jointCount; ++i) + body->joints[i].position = TPE_vec3Plus(body->joints[i].position, + offset); +} + +void TPE_bodyApplyGravity(TPE_Body *body, TPE_Unit downwardsAccel) +{ + if ((body->flags & TPE_BODY_FLAG_DEACTIVATED) || + (body->flags & TPE_BODY_FLAG_DISABLED)) + return; + + for (uint16_t i = 0; i < body->jointCount; ++i) + body->joints[i].velocity[1] -= downwardsAccel; +} + +void TPE_bodyAccelerate(TPE_Body *body, TPE_Vec3 velocity) +{ + TPE_bodyActivate(body); + + for (uint16_t i = 0; i < body->jointCount; ++i) + { + body->joints[i].velocity[0] += velocity.x; + body->joints[i].velocity[1] += velocity.y; + body->joints[i].velocity[2] += velocity.z; + } +} + +void TPE_bodyStop(TPE_Body *body) +{ + for (uint16_t i = 0; i < body->jointCount; ++i) + { + body->joints[i].velocity[0] = 0; + body->joints[i].velocity[1] = 0; + body->joints[i].velocity[2] = 0; + } +} + +void _TPE_bodyNonrotatingJointCollided(TPE_Body *b, int16_t jointIndex, + TPE_Vec3 origPos, uint8_t success) +{ + origPos = TPE_vec3Minus(b->joints[jointIndex].position,origPos); + + for (uint16_t i = 0; i < b->jointCount; ++i) + if (i != jointIndex) + { + b->joints[i].position = TPE_vec3Plus(b->joints[i].position,origPos); + + if (success) + for (uint8_t j = 0; j < 3; ++j) + b->joints[i].velocity[j] = b->joints[jointIndex].velocity[j]; + } +} + +TPE_Unit TPE_vec3Dot(TPE_Vec3 v1, TPE_Vec3 v2) +{ + return (v1.x * v2.x + v1.y * v2.y + v1.z * v2.z) / TPE_F; +} + +TPE_Unit TPE_cos(TPE_Unit x) +{ + return TPE_sin(x + TPE_F / 4); +} + +TPE_Unit TPE_sin(TPE_Unit x) +{ + int8_t sign = 1; + + if (x < 0) // odd function + { + x *= -1; + sign = -1; + } + + x %= TPE_F; + + if (x > TPE_F / 2) + { + x -= TPE_F / 2; + sign *= -1; + } + + TPE_Unit tmp = TPE_F - 2 * x; + + #define _PI2 5053 // 9.8696044 * TPE_F + return sign * // Bhaskara's approximation + (((32 * x * _PI2) / TPE_F) * tmp) / + ((_PI2 * (5 * TPE_F - (8 * x * tmp) / + TPE_F)) / TPE_F); + #undef _PI2 +} + +uint8_t TPE_bodiesResolveCollision(TPE_Body *b1, TPE_Body *b2, + TPE_ClosestPointFunction env) +{ + uint8_t r = 0; + + for (uint16_t i = 0; i < b1->jointCount; ++i) + for (uint16_t j = 0; j < b2->jointCount; ++j) + { + TPE_Vec3 origPos2 = b2->joints[j].position; + TPE_Vec3 origPos1 = b1->joints[i].position; + + _TPE_joint1Index = i; + _TPE_joint2Index = j; + + if (TPE_jointsResolveCollision(&(b1->joints[i]),&(b2->joints[j]), + b1->jointMass,b2->jointMass,(b1->elasticity + b2->elasticity) / 2, + (b1->friction + b2->friction) / 2,env)) + { + r = 1; + + if (b1->flags & TPE_BODY_FLAG_NONROTATING) + _TPE_bodyNonrotatingJointCollided(b1,i,origPos1,1); + + if (b2->flags & TPE_BODY_FLAG_NONROTATING) + _TPE_bodyNonrotatingJointCollided(b2,j,origPos2,1); + } + } + + return r; +} + +uint8_t TPE_jointsResolveCollision(TPE_Joint *j1, TPE_Joint *j2, + TPE_Unit mass1, TPE_Unit mass2, TPE_Unit elasticity, TPE_Unit friction, + TPE_ClosestPointFunction env) +{ + TPE_Vec3 dir = TPE_vec3Minus(j2->position,j1->position); + + TPE_Unit d = TPE_LENGTH(dir) - TPE_JOINT_SIZE(*j1) - TPE_JOINT_SIZE(*j2); + + if (d < 0) // collision? + { + if (_TPE_collisionCallback != 0 && !_TPE_collisionCallback( + _TPE_body1Index,_TPE_joint1Index,_TPE_body2Index,_TPE_joint2Index, + TPE_vec3Plus(j1->position,dir))) + return 0; + + TPE_Vec3 + pos1Backup = j1->position, + pos2Backup = j2->position; + + // separate joints, the shift distance will depend on the weight ratio: + + d = -1 * d + TPE_COLLISION_RESOLUTION_MARGIN; + + TPE_vec3Normalize(&dir); + + TPE_Unit ratio = (mass2 * TPE_F) / + TPE_nonZero(mass1 + mass2); + + TPE_Unit shiftDistance = (ratio * d) / TPE_F; + + TPE_Vec3 shift = TPE_vec3Times(dir,shiftDistance); + + j1->position = TPE_vec3Minus(j1->position,shift); + + shiftDistance = d - shiftDistance; + + shift = TPE_vec3Times(dir,shiftDistance); + + j2->position = TPE_vec3Plus(j2->position,shift); + + // compute new velocities: + + TPE_Unit v1, v2; + + TPE_Vec3 vel = TPE_vec3(j1->velocity[0],j1->velocity[1],j1->velocity[2]); + + vel = TPE_vec3Project(vel,dir); + + j1->velocity[0] = j1->velocity[0] - vel.x; + j1->velocity[1] = j1->velocity[1] - vel.y; + j1->velocity[2] = j1->velocity[2] - vel.z; + + /* friction explanation: Not physically correct (doesn't depend on load), + friction basically means we weighted average the velocities of the bodies + in the direction perpendicular to the hit normal, in the ratio of their + masses, friction coefficient just says how much of this effect we apply + (it multiplies the friction vectors we are subtracting) */ + + TPE_Vec3 frictionVec = + TPE_vec3(j1->velocity[0],j1->velocity[1],j1->velocity[2]); + + v1 = TPE_vec3Dot(vel,dir); + vel = TPE_vec3(j2->velocity[0],j2->velocity[1],j2->velocity[2]); + vel = TPE_vec3Project(vel,dir); + + j2->velocity[0] = j2->velocity[0] - vel.x; + j2->velocity[1] = j2->velocity[1] - vel.y; + j2->velocity[2] = j2->velocity[2] - vel.z; + + frictionVec = TPE_vec3Minus( + TPE_vec3(j2->velocity[0],j2->velocity[1],j2->velocity[2]), + frictionVec); + + v2 = TPE_vec3Dot(vel,dir); + + TPE_getVelocitiesAfterCollision(&v1,&v2,mass1,mass2,elasticity); + + vel = TPE_vec3Times(dir,v1); + +#define assignVec(j,i,d,o) \ + j->velocity[i] = j->velocity[i] + vel.d o (((frictionVec.d * ratio) / \ + TPE_F) * friction) / TPE_F; + + assignVec(j1,0,x,+) + assignVec(j1,1,y,+) + assignVec(j1,2,z,+) + + vel = TPE_vec3Times(dir,v2); + + ratio = TPE_F - ratio; + + assignVec(j2,0,x,-) + assignVec(j2,1,y,-) + assignVec(j2,2,z,-) + +#undef assignVec + + if (env != 0) + { + // ensure the joints aren't colliding with environment + + if (TPE_jointEnvironmentResolveCollision(j1,elasticity,friction,env) == 2) + j1->position = pos1Backup; + + if (TPE_jointEnvironmentResolveCollision(j2,elasticity,friction,env) == 2) + j2->position = pos2Backup; + } + + return 1; + } + + return 0; +} + +TPE_Vec3 TPE_vec3Times(TPE_Vec3 v, TPE_Unit units) +{ + v.x = (v.x * units) / TPE_F; + v.y = (v.y * units) / TPE_F; + v.z = (v.z * units) / TPE_F; + + return v; +} + +TPE_Vec3 TPE_vec3TimesPlain(TPE_Vec3 v, TPE_Unit q) +{ + v.x *= q; + v.y *= q; + v.z *= q; + + return v; +} + +void TPE_getVelocitiesAfterCollision(TPE_Unit *v1, TPE_Unit *v2, + TPE_Unit m1, TPE_Unit m2, TPE_Unit elasticity) +{ + /* In the following a lot of TPE_F cancel out, feel free to + check if confused. */ + + TPE_Unit m1Pm2 = TPE_nonZero(m1 + m2); + TPE_Unit v2Mv1 = TPE_nonZero(*v2 - *v1); + + TPE_Unit m1v1Pm2v2 = ((m1 * *v1) + (m2 * *v2)); + + *v1 = (((elasticity * m2 / TPE_F) * v2Mv1) + + m1v1Pm2v2) / m1Pm2; + + *v2 = (((elasticity * m1 / TPE_F) * -1 * v2Mv1) + + m1v1Pm2v2) / m1Pm2; +} + +uint8_t TPE_jointEnvironmentResolveCollision(TPE_Joint *joint, + TPE_Unit elasticity, TPE_Unit friction, TPE_ClosestPointFunction env) +{ + TPE_Vec3 toJoint = + TPE_vec3Minus(joint->position,env(joint->position,TPE_JOINT_SIZE(*joint))); + + TPE_Unit len = TPE_LENGTH(toJoint); + + if (len <= TPE_JOINT_SIZE(*joint)) + { + if (_TPE_collisionCallback != 0) + if (!_TPE_collisionCallback(_TPE_body1Index, + _TPE_joint1Index,_TPE_body2Index,_TPE_joint2Index, + TPE_vec3Minus(joint->position,toJoint))) + return 0; + + // colliding + + TPE_Vec3 positionBackup = joint->position, shift; + uint8_t success = 0; + + if (len > 0) + { + /* Joint center is still outside the geometry so we can determine the + normal and use it to shift it outside. This can still leave the joint + colliding though, so try to repeat it a few times. */ + + for (int i = 0; i < TPE_COLLISION_RESOLUTION_ITERATIONS; ++i) + { + shift = toJoint; + + TPE_vec3Normalize(&shift); + + shift = TPE_vec3Times(shift,TPE_JOINT_SIZE(*joint) - len + + TPE_COLLISION_RESOLUTION_MARGIN); + + joint->position = TPE_vec3Plus(joint->position,shift); + + toJoint = TPE_vec3Minus(joint->position,env(joint->position, + TPE_JOINT_SIZE(*joint))); + + len = TPE_LENGTH(toJoint); // still colliding? + + if (len >= TPE_JOINT_SIZE(*joint)) + { + success = 1; + break; + } + } + } + + if (!success) + { + /* Shifting along normal was unsuccessfull, now try different approach: + shift back by joint velocity. */ + + shift = TPE_vec3(-1 * joint->velocity[0],-1 * joint->velocity[1], + -1 * joint->velocity[2]); + + for (int i = 0; i < TPE_COLLISION_RESOLUTION_ITERATIONS; ++i) + { + joint->position = TPE_vec3Plus(joint->position,shift); + + toJoint = TPE_vec3Minus(joint->position, + env(joint->position,TPE_JOINT_SIZE(*joint))); + + len = TPE_LENGTH(toJoint); // still colliding? + + if (len >= TPE_JOINT_SIZE(*joint)) + { + success = 1; + break; + } + + shift.x /= 2; // decrease the step a bit + shift.y /= 2; + shift.z /= 2; + } + } + + if (success) + { + TPE_Vec3 vel = TPE_vec3(joint->velocity[0],joint->velocity[1], + joint->velocity[2]); + + vel = TPE_vec3Project(vel,shift); // parallel part of velocity + + TPE_Vec3 vel2 = TPE_vec3Minus( // perpendicular part of velocity + TPE_vec3(joint->velocity[0],joint->velocity[1],joint->velocity[2]),vel); + + vel2 = TPE_vec3Times(vel2,friction); + + vel = TPE_vec3Times(vel,TPE_F + elasticity); + + joint->velocity[0] -= vel.x + vel2.x; + joint->velocity[1] -= vel.y + vel2.y; + joint->velocity[2] -= vel.z + vel2.z; + } + else + { + TPE_LOG("WARNING: joint-environment collision couldn't be resolved"); + + joint->position = positionBackup; + joint->velocity[0] = 0; + joint->velocity[1] = 0; + joint->velocity[2] = 0; + + return 2; + } + + return 1; + } + + return 0; +} + +uint8_t TPE_bodyEnvironmentCollide(const TPE_Body *body, + TPE_ClosestPointFunction env) +{ + for (uint16_t i = 0; i < body->jointCount; ++i) + { + const TPE_Joint *joint = body->joints + i; + + TPE_Unit size = TPE_JOINT_SIZE(*joint); + + if (TPE_DISTANCE(joint->position,env(joint->position,size)) <= size) + return 1; + } + + return 0; +} + +void TPE_bodyGetFastBSphere(const TPE_Body *body, TPE_Vec3 *center, + TPE_Unit *radius) +{ + TPE_Vec3 b; + + TPE_bodyGetAABB(body,center,&b); + + center->x = (center->x + b.x) / 2; + center->y = (center->y + b.y) / 2; + center->z = (center->z + b.z) / 2; + + *radius = TPE_DISTANCE(*center,b); +} + +void TPE_bodyGetBSphere(const TPE_Body *body, TPE_Vec3 *center, + TPE_Unit *radius) +{ + *radius = TPE_INFINITY; + *center = TPE_bodyGetCenterOfMass(body); + + const TPE_Joint *j = body->joints; + + for (uint16_t i = 0; i < body->jointCount; ++i) + { + TPE_Vec3 diff; + + TPE_Unit js = TPE_JOINT_SIZE(*j); + + /* Sadly we have to have these conditions here which slow this down. If we + were only computing a BB sphere of a point cloud, we wouldn't have to + compute abs vals (as squaring would effectively compute them), but here + we need to add joint size which needs to know about the sign. */ + + diff.x = ((center->x > j->position.x) ? + (center->x - j->position.x) : (j->position.x - center->x)) + js; + + diff.y = ((center->y > j->position.y) ? + (center->y - j->position.y) : (j->position.y - center->y)) + js; + + diff.z = ((center->z > j->position.z) ? + (center->z - j->position.z) : (j->position.z - center->z)) + js; + + TPE_Unit distSquared = + diff.x * diff.x + diff.y * diff.y + diff.z * diff.z; + + if (distSquared < *radius) + *radius = distSquared; + + j++; + } + + *radius = TPE_sqrt(*radius); +} + +uint8_t TPE_bodyEnvironmentResolveCollision(TPE_Body *body, + TPE_ClosestPointFunction env) +{ + TPE_Vec3 c; + TPE_Unit d; + + TPE_bodyGetFastBSphere(body,&c,&d); + + if (TPE_DISTANCE(c,env(c,d)) > d) + return 0; + + // now test the full body collision: + + uint8_t collision = 0; + + for (uint16_t i = 0; i < body->jointCount; ++i) + { + TPE_Vec3 previousPos = body->joints[i].position; + + _TPE_joint1Index = i; + + uint8_t r = TPE_jointEnvironmentResolveCollision( + body->joints + i,body->elasticity,body->friction,env); + + if (r) + { + collision = 1; + + if (body->flags & TPE_BODY_FLAG_NONROTATING) + _TPE_bodyNonrotatingJointCollided(body,i,previousPos,r == 1); + } + } + + return collision; +} + +TPE_Vec3 TPE_vec3Normalized(TPE_Vec3 v) +{ + TPE_vec3Normalize(&v); + return v; +} + +TPE_Unit TPE_atan(TPE_Unit x) +{ + /* atan approximation by polynomial + WARNING: this will break with different value of TPE_FRACTIONS_PER_UNIT */ + + TPE_Unit sign = 1, x2 = x * x; + + if (x < 0) + { + x *= -1; + sign = -1; + } + + if (x > 30000) // anti overflow + return sign * (TPE_F / 4); + + return sign * + (307 * x + x2) / ((267026 + 633 * x + x2) / 128); +} + +void _TPE_vec2Rotate(TPE_Unit *x, TPE_Unit *y, TPE_Unit angle) +{ + TPE_Unit tmp = *x; + + TPE_Unit s = TPE_sin(angle); + TPE_Unit c = TPE_cos(angle); + + *x = (c * *x - s * *y) / TPE_F; + *y = (s * tmp + c * *y) / TPE_F; +} + +TPE_Unit TPE_vec2Angle(TPE_Unit x, TPE_Unit y) +{ + TPE_Unit r = 0; + + if (x != 0) + { + r = TPE_atan((y * TPE_F) / x); + + if (x < 0) + r += TPE_F / 2; + else if (r < 0) + r += TPE_F; + } + else + { + if (y < 0) + r = (3 * TPE_F) / 4; + else if (y > 0) + r = TPE_F / 4; + // else (y == 0) r stays 0 + } + + return r; +} + +TPE_Vec3 TPE_rotationFromVecs(TPE_Vec3 forward, TPE_Vec3 right) +{ + TPE_Vec3 result; + + // get rotation around Y: + + result.y = TPE_vec2Angle(forward.z,-1 * forward.x); + + // now rotate back by this angle to align with x = 0 plane: + + _TPE_vec2Rotate(&forward.z,&forward.x,result.y); + _TPE_vec2Rotate(&right.z,&right.x,result.y); + + // now do the same for the second axis: + + result.x = + TPE_vec2Angle(forward.z,forward.y); + + _TPE_vec2Rotate(&right.z,&right.y,-1 * result.x); + + result.z = TPE_vec2Angle(right.x,-1 * right.y); + + return result; +} + +TPE_Vec3 _TPE_project3DPoint(TPE_Vec3 p, TPE_Vec3 camPos, TPE_Vec3 camRot, + TPE_Vec3 camView) +{ + // transform to camera space: + + p = TPE_vec3Minus(p,camPos); + + _TPE_vec2Rotate(&p.z,&p.x,camRot.y); + _TPE_vec2Rotate(&p.z,&p.y,-1 * camRot.x); + _TPE_vec2Rotate(&p.y,&p.x,-1 * camRot.z); + + if (p.z <= 0) + return p; + + if (camView.z != 0) + { + // perspective + + p.x = (p.x * camView.z) / p.z; + p.y = (p.y * camView.z) / p.z; + + p.x = camView.x / 2 + (p.x * camView.x) / (2 * TPE_F); + p.y = camView.y / 2 - (p.y * camView.x) / (2 * TPE_F); + // ^ x here intentional + } + else + { + // ortho + + p.x = camView.x / 2 + p.x; + p.y = camView.y / 2 - p.y; + } + + return p; +} + +void _TPE_drawDebugPixel( + TPE_Unit x, TPE_Unit y, TPE_Unit w, TPE_Unit h, uint8_t c, + TPE_DebugDrawFunction f) +{ + if (x >= 0 && x < w && y >= 0 && y < h) + f(x,y,c); +} + +void TPE_worldDebugDraw(TPE_World *world, TPE_DebugDrawFunction drawFunc, + TPE_Vec3 camPos, TPE_Vec3 camRot, TPE_Vec3 camView, uint16_t envGridRes, + TPE_Unit envGridSize) +{ +#define Z_LIMIT 250 + if (world->environmentFunction != 0) + { + // environment: + + TPE_Vec3 testPoint; + + TPE_Unit gridHalfSize = (envGridSize * envGridRes) / 2; + + TPE_Vec3 center; + + if (envGridRes != 0) + { + center = TPE_vec3(0,TPE_sin(camRot.x),TPE_cos(camRot.x)); + + _TPE_vec2Rotate(¢er.x,¢er.z,camRot.y); + + center = TPE_vec3Times(center,gridHalfSize); + center = TPE_vec3Plus(camPos,center); + + center.x = (center.x / envGridSize) * envGridSize; + center.y = (center.y / envGridSize) * envGridSize; + center.z = (center.z / envGridSize) * envGridSize; + } + + testPoint.y = center.y - gridHalfSize; + + for (uint8_t j = 0; j < envGridRes; ++j) + { + testPoint.x = center.x - gridHalfSize; + + for (uint8_t k = 0; k < envGridRes; ++k) + { + testPoint.z = center.z - gridHalfSize; + + for (uint8_t l = 0; l < envGridRes; ++l) + { + TPE_Vec3 r = world->environmentFunction(testPoint,envGridSize); + + if (r.x != testPoint.x || r.y != testPoint.y || r.z != testPoint.z) + { + r = _TPE_project3DPoint(r,camPos,camRot,camView); + + if (r.z > Z_LIMIT) + _TPE_drawDebugPixel(r.x,r.y,camView.x,camView.y, + TPE_DEBUG_COLOR_ENVIRONMENT,drawFunc); + } + + testPoint.z += envGridSize; + } + + testPoint.x += envGridSize; + } + + testPoint.y += envGridSize; + } + } + + for (uint16_t i = 0; i < world->bodyCount; ++i) + { + // connections: + for (uint16_t j = 0; j < world->bodies[i].connectionCount; ++j) + { + TPE_Vec3 + p1 = world->bodies[i].joints[ + world->bodies[i].connections[j].joint1].position, + p2 = world->bodies[i].joints[ + world->bodies[i].connections[j].joint2].position; + + p1 = _TPE_project3DPoint(p1,camPos,camRot,camView); + p2 = _TPE_project3DPoint(p2,camPos,camRot,camView); + + if (p1.z <= Z_LIMIT || p2.z <= Z_LIMIT) + continue; + + TPE_Vec3 diff = TPE_vec3Minus(p2,p1); + +#define SEGS 16 + + uint8_t c = (world->bodies[i].flags & TPE_BODY_FLAG_DEACTIVATED) ? + TPE_DEBUG_COLOR_INACTIVE : TPE_DEBUG_COLOR_CONNECTION; + + for (uint16_t k = 0; k < SEGS; ++k) + { + p2.x = p1.x + (diff.x * k) / SEGS; + p2.y = p1.y + (diff.y * k) / SEGS; + + _TPE_drawDebugPixel(p2.x,p2.y,camView.x,camView.y,c,drawFunc); + } +#undef SEGS + } + + // joints: + for (uint16_t j = 0; j < world->bodies[i].jointCount; ++j) + { + TPE_Vec3 p = _TPE_project3DPoint(world->bodies[i].joints[j].position, + camPos,camRot,camView); + + if (p.z > Z_LIMIT) + { + uint8_t color = (world->bodies[i].flags & TPE_BODY_FLAG_DEACTIVATED) ? + TPE_DEBUG_COLOR_INACTIVE : TPE_DEBUG_COLOR_JOINT; + + _TPE_drawDebugPixel(p.x,p.y,camView.x,camView.y,color,drawFunc); + + TPE_Unit size = TPE_JOINT_SIZE(world->bodies[i].joints[j]); + + if (camView.z != 0) // not ortho? + { + size /= 2; + size = (size * camView.x) / TPE_F; + size = (size * camView.z) / p.z; + } + +#define SEGS 4 + for (uint8_t k = 0; k < SEGS + 1; ++k) + { + TPE_Unit + dx = (TPE_sin(TPE_F * k / (8 * SEGS)) * size) + / TPE_F, + dy = (TPE_cos(TPE_F * k / (8 * SEGS)) * size) + / TPE_F; + +#define dp(a,b,c,d) \ + _TPE_drawDebugPixel(p.x a b,p.y c d,camView.x,camView.y,color,drawFunc); + dp(+,dx,+,dy) dp(+,dx,-,dy) dp(-,dx,+,dy) dp(-,dx,-,dy) + dp(+,dy,+,dx) dp(+,dy,-,dx) dp(-,dy,+,dx) dp(-,dy,-,dx) +#undef dp +#undef SEGS + } + } + } + } +#undef Z_LIMIT +} + +TPE_Vec3 TPE_envBox(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 maxCornerVec, + TPE_Vec3 rotation) +{ + point = TPE_pointRotate(TPE_vec3Minus(point,center), + TPE_rotationInverse(rotation)); + + return TPE_vec3Plus(center,TPE_pointRotate(TPE_envAABox(point,TPE_vec3(0,0,0), + maxCornerVec),rotation)); +} + +TPE_Vec3 TPE_envAABox(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 maxCornerVec) +{ + TPE_Vec3 shifted = TPE_vec3Minus(point,center); + int8_t sign[3] = {1, 1, 1}; + + if (shifted.x < 0) + { + shifted.x *= -1; + sign[0] = -1; + } + + if (shifted.y < 0) + { + shifted.y *= -1; + sign[1] = -1; + } + + if (shifted.z < 0) + { + shifted.z *= -1; + sign[2] = -1; + } + + uint8_t region = + (shifted.x > maxCornerVec.x) | + ((shifted.y > maxCornerVec.y) << 1) | + ((shifted.z > maxCornerVec.z) << 2); + + switch (region) + { +#define align(c,i) point.c = center.c + sign[i] * maxCornerVec.c + + case 0x01: align(x,0); break; + case 0x02: align(y,1); break; + case 0x04: align(z,2); break; + + case 0x03: align(x,0); align(y,1); break; + case 0x05: align(x,0); align(z,2); break; + case 0x06: align(y,1); align(z,2); break; + + case 0x07: align(x,0); align(y,1); align(z,2); break; + default: break; + +#undef align + } + + return point; +} + +TPE_Vec3 TPE_envAABoxInside(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 size) +{ + size.x /= 2; + size.y /= 2; + size.z /= 2; + + TPE_Vec3 shifted = TPE_vec3Minus(point,center); + + TPE_Vec3 a = TPE_vec3Minus(size,shifted), + b = TPE_vec3Plus(shifted,size); + + int8_t sx = 1, sy = 1, sz = 1; + + if (b.x < a.x) + { + a.x = b.x; + sx = -1; + } + + if (b.y < a.y) + { + a.y = b.y; + sy = -1; + } + + if (b.z < a.z) + { + a.z = b.z; + sz = -1; + } + + if (a.x < 0 || a.y < 0 || a.z < 0) + return point; + + if (a.x < a.y) + { + if (a.x < a.z) + point.x = center.x + sx * size.x; + else + point.z = center.z + sz * size.z; + } + else + { + if (a.y < a.z) + point.y = center.y + sy * size.y; + else + point.z = center.z + sz * size.z; + } + + return point; +} + +TPE_Vec3 TPE_envSphereInside(TPE_Vec3 point, TPE_Vec3 center, TPE_Unit radius) +{ + TPE_Vec3 shifted = TPE_vec3Minus(point,center); + + TPE_Unit l = TPE_LENGTH(shifted); + + if (l >= radius) + return point; + else if (l < 0) + return TPE_vec3(center.x + radius,center.y,center.z); + + TPE_vec3Normalize(&shifted); + + return TPE_vec3Plus(center,TPE_vec3Times(shifted,radius)); +} + +TPE_Vec3 TPE_envSphere(TPE_Vec3 point, TPE_Vec3 center, TPE_Unit radius) +{ + TPE_Vec3 dir = TPE_vec3Minus(point,center); + + TPE_Unit l = TPE_LENGTH(dir); + + if (l <= radius) + return point; + + dir.x = (dir.x * radius) / l; + dir.y = (dir.y * radius) / l; + dir.z = (dir.z * radius) / l; + + return TPE_vec3Plus(center,dir); +} + +TPE_Vec3 TPE_envHalfPlane(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 normal) +{ + TPE_Vec3 point2 = TPE_vec3Minus(point,center); + + TPE_Unit tmp = + point2.x * normal.x + point2.y * normal.y + point2.z * normal.z; + + if (tmp < 0) + return point; + + TPE_Unit l = TPE_LENGTH(normal); + + tmp /= l; + + normal.x = (normal.x * TPE_F) / l; + normal.y = (normal.y * TPE_F) / l; + normal.z = (normal.z * TPE_F) / l; + + return TPE_vec3Minus(point, + TPE_vec3Times(normal,tmp)); +} + +uint8_t TPE_checkOverlapAABB(TPE_Vec3 v1Min, TPE_Vec3 v1Max, TPE_Vec3 v2Min, + TPE_Vec3 v2Max) +{ + TPE_Unit dist; + +#define test(c) \ + dist = v1Min.c + v1Max.c - v2Max.c - v2Min.c; \ + if (dist < 0) dist *= -1; \ + if (dist > v1Max.c - v1Min.c + v2Max.c - v2Min.c) return 0; + + test(x) + test(y) + test(z) + +#undef test + + return 1; +} + +void TPE_bodyGetAABB(const TPE_Body *body, TPE_Vec3 *vMin, TPE_Vec3 *vMax) +{ + *vMin = body->joints[0].position; + *vMax = *vMin; + + TPE_Unit js = TPE_JOINT_SIZE(body->joints[0]); + + vMin->x -= js; + vMin->y -= js; + vMin->z -= js; + + vMax->x += js; + vMax->y += js; + vMax->z += js; + + for (uint16_t i = 1; i < body->jointCount; ++i) + { + TPE_Unit v; + + js = TPE_JOINT_SIZE(body->joints[i]); + +#define test(c) \ + v = body->joints[i].position.c - js; \ + if (v < vMin->c) \ + vMin->c = v; \ + v += 2 * js; \ + if (v > vMax->c) \ + vMax->c = v; + + test(x) + test(y) + test(z) + +#undef test + } +} + +void TPE_jointPin(TPE_Joint *joint, TPE_Vec3 position) +{ + joint->position = position; + joint->velocity[0] = 0; + joint->velocity[1] = 0; + joint->velocity[2] = 0; +} + +TPE_Vec3 TPE_pointRotate(TPE_Vec3 point, TPE_Vec3 rotation) +{ + _TPE_vec2Rotate(&point.y,&point.x,rotation.z); + _TPE_vec2Rotate(&point.z,&point.y,rotation.x); + _TPE_vec2Rotate(&point.x,&point.z,rotation.y); + + return point; +} + +TPE_Vec3 TPE_rotationInverse(TPE_Vec3 rotation) +{ + /* If r1 = (X,Y,Z) is rotation in convention ABC then r1^-1 = (-X,-Y,-Z) in + convention CBA is its inverse rotation. We exploit this, i.e. we rotate + forward/right vectors in opposite axis order and then turn the result + into normal rotation/orientation. */ + + TPE_Vec3 f = TPE_vec3(0,0,TPE_F); + TPE_Vec3 r = TPE_vec3(TPE_F,0,0); + + rotation.x *= -1; + rotation.y *= -1; + rotation.z *= -1; + + _TPE_vec2Rotate(&f.x,&f.z,rotation.y); + _TPE_vec2Rotate(&f.z,&f.y,rotation.x); + _TPE_vec2Rotate(&f.y,&f.x,rotation.z); + + _TPE_vec2Rotate(&r.x,&r.z,rotation.y); + _TPE_vec2Rotate(&r.z,&r.y,rotation.x); + _TPE_vec2Rotate(&r.y,&r.x,rotation.z); + + return TPE_rotationFromVecs(f,r); +} + +TPE_Vec3 TPE_rotationRotateByAxis(TPE_Vec3 rotation, TPE_Vec3 rotationByAxis) +{ + TPE_Vec3 f = TPE_pointRotate(TPE_vec3(0,0,TPE_F),rotation); + TPE_Vec3 r = TPE_pointRotate(TPE_vec3(TPE_F,0,0),rotation); + + TPE_Unit a = TPE_LENGTH(rotationByAxis); + TPE_vec3Normalize(&rotationByAxis); + + f = _TPE_rotateByAxis(f,rotationByAxis,a); + r = _TPE_rotateByAxis(r,rotationByAxis,a); + + return TPE_rotationFromVecs(f,r); +} + +TPE_Unit TPE_keepInRange(TPE_Unit x, TPE_Unit xMin, TPE_Unit xMax) +{ + return x > xMin ? (x < xMax ? x : xMax) : xMin; +} + +TPE_Vec3 TPE_vec3KeepWithinDistanceBand(TPE_Vec3 point, TPE_Vec3 center, + TPE_Unit minDistance, TPE_Unit maxDistance) +{ + TPE_Vec3 toPoint = TPE_vec3Minus(point,center); + + TPE_Unit l = TPE_LENGTH(toPoint); + + if (l <= maxDistance) + { + if (l >= minDistance) + return point; + + l = minDistance; + } + else + l = maxDistance; + + return TPE_vec3Plus(center, + TPE_vec3Times(TPE_vec3Normalized(toPoint),l)); +} + +TPE_Vec3 TPE_vec3KeepWithinBox(TPE_Vec3 point, TPE_Vec3 boxCenter, + TPE_Vec3 boxMaxVect) +{ + point.x = TPE_keepInRange(point.x, + boxCenter.x - boxMaxVect.x,boxCenter.x + boxMaxVect.x); + + point.y = TPE_keepInRange(point.y, + boxCenter.y - boxMaxVect.y,boxCenter.y + boxMaxVect.y); + + point.z = TPE_keepInRange(point.z, + boxCenter.z - boxMaxVect.z,boxCenter.z + boxMaxVect.z); + + return point; +} + +TPE_Vec3 TPE_envInfiniteCylinder(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 + direction, TPE_Unit radius) +{ + TPE_Vec3 d = TPE_vec3Minus(point,center); + d = TPE_vec3Minus(d,TPE_vec3Project(d,direction)); + + TPE_Unit l = TPE_LENGTH(d); + + if (l <= radius) + return point; + + radius = l - radius; + + d.x = (d.x * radius) / l; + d.y = (d.y * radius) / l; + d.z = (d.z * radius) / l; + + return TPE_vec3Minus(point,d); +} + +TPE_Vec3 TPE_envCylinder(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 direction, + TPE_Unit radius) +{ + point = TPE_vec3Minus(point,center); + + TPE_Vec3 projected = TPE_vec3Project(point,direction); + + point = TPE_envInfiniteCylinder(point,TPE_vec3(0,0,0),direction,radius); + + TPE_Unit lDir = TPE_nonZero(TPE_LENGTH(direction)); + + TPE_Unit lDiff = TPE_LENGTH(projected) - lDir; + + if (lDiff > 0) + { + direction.x = (direction.x * lDiff) / lDir; + direction.y = (direction.y * lDiff) / lDir; + direction.z = (direction.z * lDiff) / lDir; + + point = (TPE_vec3Dot(projected,direction)) >= 0 ? + TPE_vec3Minus(point,direction) : TPE_vec3Plus(point,direction); + } + + return TPE_vec3Plus(center,point); +} + +TPE_Vec3 TPE_fakeSphereRotation(TPE_Vec3 position1, TPE_Vec3 position2, + TPE_Unit radius) +{ + TPE_Vec3 m; + + m.x = position1.z - position2.z; + m.y = 0; + m.z = position2.x - position1.x; + + TPE_Unit l = TPE_sqrt(m.x * m.x + m.z * m.z); + + if (l == 0) + return TPE_vec3(0,0,0); + + TPE_Unit d = (TPE_DISTANCE(position1,position2) * + TPE_F) / (radius * 4); + + m.x = (m.x * d) / l; + m.z = (m.z * d) / l; + + return m; +} + +TPE_Vec3 TPE_castEnvironmentRay(TPE_Vec3 rayPos, TPE_Vec3 rayDir, + TPE_ClosestPointFunction environment, TPE_Unit insideStepSize, + TPE_Unit rayMarchMaxStep, uint32_t maxSteps) +{ + TPE_Vec3 p = rayPos; + TPE_Vec3 p2 = environment(rayPos,rayMarchMaxStep); + TPE_Unit totalD = 0; + + TPE_vec3Normalize(&rayDir); + + uint8_t found = 0; // 0 = nothing found, 1 = out/in found, 2 = in/out found + + if (p2.x != p.x || p2.y != p.y || p2.z != p.z) + { + // outside ray: ray march + + for (uint32_t i = 0; i < maxSteps; ++i) + { + TPE_Unit d = TPE_DISTANCE(p,p2); + + if (d > rayMarchMaxStep) + d = rayMarchMaxStep; + + totalD += d; + + p2 = TPE_vec3Plus(rayPos,TPE_vec3Times(rayDir,totalD)); + + if (d == 0 || + (p2.x == p.x && p2.y == p.y && p2.z == p.z)) + return p2; // point not inside env but dist == 0, ideal case + + TPE_Vec3 pTest = environment(p2,rayMarchMaxStep); + + if (pTest.x == p2.x && pTest.y == p2.y && pTest.z == p2.z) + { + // stepped into env, will have to iterate + found = 1; + break; + } + + p = p2; + p2 = pTest; + } + } + else if (insideStepSize != 0) + { + // inside ray: iterate by fixed steps + + for (uint32_t i = 0; i < maxSteps; ++i) + { + totalD += insideStepSize; + + p2 = TPE_vec3Plus(rayPos,TPE_vec3Times(rayDir,totalD)); + + TPE_Vec3 pTest = environment(p2,16); + + if (p2.x != pTest.x || p2.y != pTest.y || p2.z != pTest.z) + { + found = 2; + break; + } + + p = p2; + p2 = pTest; + } + } + + if (found) + { + /* Here we've found two points (p, p2), each one the other side of the + env surface. Now iterate (binary search) to find the exact surface + pos. */ + + for (uint8_t i = 0; i < 128; ++i) // upper limit just in case + { + TPE_Vec3 middle = TPE_vec3Plus(p,p2); + + middle.x /= 2; + middle.y /= 2; + middle.z /= 2; + + if ((middle.x == p.x && middle.y == p.y && middle.z == p.z) || + (middle.x == p2.x && middle.y == p2.y && middle.z == p2.z)) + break; // points basically next to each other, don't continue + + TPE_Vec3 pTest = environment(middle,16); // 16: just a small number + + if ((found == 1) == + (pTest.x == middle.x && pTest.y == middle.y && pTest.z == middle.z)) + p2 = middle; + else + p = middle; + } + + return (found == 1) ? p : p2; + } + + return TPE_vec3(TPE_INFINITY,TPE_INFINITY,TPE_INFINITY); +} + +TPE_Vec3 TPE_castBodyRay(TPE_Vec3 rayPos, TPE_Vec3 rayDir, int16_t excludeBody, + const TPE_World *world, int16_t *bodyIndex, int16_t *jointIndex) +{ + TPE_Vec3 bestP = TPE_vec3(TPE_INFINITY,TPE_INFINITY,TPE_INFINITY); + TPE_Unit bestD = TPE_INFINITY; + + if (bodyIndex != 0) + *bodyIndex = -1; + + if (jointIndex != 0) + *jointIndex = -1; + + TPE_vec3Normalize(&rayDir); + + for (uint16_t i = 0; i < world->bodyCount; ++i) + { + TPE_Vec3 c, p; + TPE_Unit r, d; + + TPE_bodyGetFastBSphere(&world->bodies[i],&c,&r); + + c = TPE_vec3Minus(c,rayPos); + p = TPE_vec3ProjectNormalized(c,rayDir); + + if (TPE_vec3Dot(p,rayDir) >= 0) // point is in ray's forward dir? + { + d = TPE_DISTANCE(p,c); + + if (d <= r) + { + // bounding sphere hit, now check all joints: + + const TPE_Joint *joint = world->bodies[i].joints; + + for (uint16_t j = 0; j < world->bodies[i].jointCount; ++j) + { + c = joint->position; + c = TPE_vec3Minus(c,rayPos); + p = TPE_vec3ProjectNormalized(c,rayDir); + + if (TPE_vec3Dot(p,rayDir) >= 0) + { + d = TPE_DISTANCE(p,c); + TPE_Unit js = TPE_JOINT_SIZE(*joint); + + if (d <= js) + { + // joint hit, compute exact coordinates: + + if (bodyIndex != 0) + *bodyIndex = i; + + if (jointIndex != 0) + *jointIndex = j; + + c = TPE_vec3Times(rayDir,TPE_sqrt(js * js - d * d)); + // ^ offset vector to two intersections + p = TPE_vec3Plus(p,rayPos); + + TPE_Vec3 + i1 = TPE_vec3Plus(p,c), // intersection points + i2 = TPE_vec3Minus(p,c); + + d = TPE_DISTANCE(rayPos,i1); + TPE_Unit d2 = TPE_DISTANCE(rayPos,i2); + + if (d2 < d) // take the closer one + { + d = d2; + i1 = i2; + } + + if (d < bestD) + { + bestD = d; + bestP = i1; + } + } + } + + joint++; + } + } + } + } + + return bestP; +} + +void TPE_worldDeactivateAll(TPE_World *world) +{ + for (uint16_t i = 0; i < world->bodyCount; ++i) + TPE_bodyDeactivate(&world->bodies[i]); +} + +void TPE_worldActivateAll(TPE_World *world) +{ + for (uint16_t i = 0; i < world->bodyCount; ++i) + TPE_bodyActivate(&world->bodies[i]); +} + +TPE_Unit TPE_worldGetNetSpeed(const TPE_World *world) +{ + TPE_Unit result = 0; + + for (uint16_t i = 0; i < world->bodyCount; ++i) + result += TPE_bodyGetNetSpeed(world->bodies + i); + + return result; +} + +TPE_Vec3 TPE_bodyGetLinearVelocity(const TPE_Body *body) +{ + TPE_Vec3 r = TPE_vec3(0,0,0); + + for (uint16_t i = 0; i < body->jointCount; ++i) + { + TPE_UnitReduced *v = body->joints[i].velocity; + r = TPE_vec3Plus(r,TPE_vec3(v[0],v[1],v[2])); + } + + r.x /= body->jointCount; + r.y /= body->jointCount; + r.z /= body->jointCount; + + return r; +} + +TPE_Unit TPE_abs(TPE_Unit x) +{ + return x >= 0 ? x : (-1 * x); +} + +TPE_Unit TPE_max(TPE_Unit a, TPE_Unit b) +{ + return (a > b) ? a : b; +} + +TPE_Unit TPE_min(TPE_Unit a, TPE_Unit b) +{ + return (a < b) ? a : b; +} + +TPE_Vec3 TPE_envAATriPrism(TPE_Vec3 point, TPE_Vec3 center, + const TPE_Unit sides[6], TPE_Unit depth, uint8_t direction) +{ + point = TPE_vec3Minus(point,center); + + if (direction == 1) + { + TPE_Unit tmp = point.z; + point.z = point.y; + point.y = tmp; + } + else if (direction == 2) + { + TPE_Unit tmp = point.z; + point.z = point.x; + point.x = tmp; + } + + depth /= 2; + + if (point.z > depth) + point.z = depth; + else + { + depth *= -1; + + if (point.z < depth) + point.z = depth; + } + + for (uint8_t i = 0; i < 6; i += 2) + { + uint8_t i2 = i < 4 ? i + 2 : 0; + + TPE_Vec3 p = + TPE_envHalfPlane(point,TPE_vec3(sides[i],sides[i + 1],0), + TPE_vec3(sides[i2 + 1] - sides[i + 1],sides[i] - sides[i2],0)); + + if (p.x != point.x || p.y != point.y) + { + point = p; + + if ( // dot product to determine which side the point is on + (sides[i2] - sides[i]) * (point.x - sides[i]) + + (sides[i2 + 1] - sides[i + 1]) * (point.y - sides[i + 1]) < 0) + { + point.x = sides[i]; point.y = sides[i + 1]; + } + else if ( // same but for the other vertex + (sides[i] - sides[i2]) * (point.x - sides[i2]) + + (sides[i + 1] - sides[i2 + 1]) * (point.y - sides[i2 + 1]) < 0) + { + point.x = sides[i2]; point.y = sides[i2 + 1]; + } + + break; + } + } + + if (direction == 1) + { + TPE_Unit tmp = point.z; + point.z = point.y; + point.y = tmp; + } + else if (direction == 2) + { + TPE_Unit tmp = point.z; + point.z = point.x; + point.x = tmp; + } + + return TPE_vec3Plus(point,center); +} + +TPE_Vec3 TPE_envGround(TPE_Vec3 point, TPE_Unit height) +{ + if (point.y > height) + point.y = height; + + return point; +} + +uint32_t _TPE_hash(uint32_t n) +{ + // parameters found by hash-prospector project + n = 250009959 * (n ^ (n >> 17)); + n = 2626308659 * (n ^ (n >> 15)); + return n ^ (n >> 16); +} + +uint32_t TPE_jointHash(const TPE_Joint *joint) +{ + uint32_t + r = _TPE_hash(joint->position.x); + r = _TPE_hash(r ^ joint->position.y); + r = _TPE_hash(r ^ joint->position.z); + r = _TPE_hash(r ^ + (((uint32_t) joint->velocity[0]) | + (((uint32_t) joint->velocity[1]) << 16))); + r = _TPE_hash(r ^ + (((uint32_t) joint->velocity[2]) | + ((uint32_t) joint->sizeDivided))); + + return r; +} + +uint32_t TPE_connectionHash(const TPE_Connection *connection) +{ + return _TPE_hash( + ((uint32_t) connection->length) | + (((uint32_t) connection->joint1) << 16) | + (((uint32_t) connection->joint2) << 24)); +} + +uint32_t TPE_bodyHash(const TPE_Body *body) +{ + uint32_t r = _TPE_hash( + ((uint32_t) body->jointMass) | + (((uint32_t) body->flags) << 16) | + (((uint32_t) body->deactivateCount) << 24)) ^ + _TPE_hash( + ((uint32_t) body->friction) | + (((uint32_t) body->elasticity) << 16)); + + for (uint8_t i = 0; i < body->jointCount; ++i) + r = _TPE_hash(r ^ TPE_jointHash(&body->joints[i])); + + for (uint8_t i = 0; i < body->connectionCount; ++i) + r = _TPE_hash(r ^ TPE_connectionHash(&body->connections[i])); + + return r; +} + +uint32_t TPE_worldHash(const TPE_World *world) +{ + uint32_t r = 0; + + for (uint8_t i = 0; i < world->bodyCount; ++i) + r = _TPE_hash(r ^ TPE_bodyHash(&world->bodies[i])); + + return r; +} + +void TPE_bodyMoveTo(TPE_Body *body, TPE_Vec3 position) +{ + position = TPE_vec3Minus(position,TPE_bodyGetCenterOfMass(body)); + + for (uint8_t i = 0; i < body->jointCount; ++i) + body->joints[i].position = TPE_vec3Plus(body->joints[i].position,position); +} + +uint8_t TPE_testClosestPointFunction(TPE_ClosestPointFunction f, + TPE_Vec3 cornerFrom, TPE_Vec3 cornerTo, uint8_t gridResolution, + TPE_UnitReduced allowedError, TPE_Vec3 *errorPoint) +{ + TPE_Vec3 p; + + cornerTo = TPE_vec3Minus(cornerTo,cornerFrom); + + for (uint16_t z = 0; z < gridResolution; ++z) + { + p.z = cornerFrom.z + (z * cornerTo.z) / gridResolution; + + for (uint16_t y = 0; y < gridResolution; ++y) + { + p.y = cornerFrom.y + (y * cornerTo.y) / gridResolution; + + for (uint16_t x = 0; x < gridResolution; ++x) + { + p.x = cornerFrom.x + (x * cornerTo.x) / gridResolution; + + TPE_Vec3 p2 = f(p,TPE_INFINITY); + + if (p.x != p2.x || p.y != p2.y || p.z != p2.z) // only test outside + { + // 1st try to approach the closest point and see if it stays the same: + + TPE_Vec3 p3 = p; + + for (uint8_t i = 0; i < 3; ++i) + { + p3 = + TPE_vec3((p3.x + p2.x) / 2,(p3.y + p2.y) / 2,(p3.z + p2.z) / 2); + + TPE_Vec3 p4 = f(p3,TPE_INFINITY); + + if (TPE_abs(p4.x - p2.x) + TPE_abs(p4.y - p2.y) + + TPE_abs(p4.z - p2.z) > allowedError) // taxicab dist. for speed + { + if (errorPoint != 0) + *errorPoint = p; + + return 0; + } + } + + // now test 8 points inside the sphere of radius: + + TPE_Unit d = TPE_DISTANCE(p,p2); + + p3.z = p.z - d / 2; + + for (uint8_t zz = 0; zz < 2; ++zz) + { + p3.y = p.y - d / 2; + + for (uint8_t yy = 0; yy < 2; ++yy) + { + p3.x = p.x - d / 2; + + for (uint8_t zz = 0; zz < 2; ++zz) + { + if (TPE_DISTANCE(p,f(p3,TPE_INFINITY)) + allowedError < d) + { + /* In the sphere of distance radius to the original point's + closest point we've gotten a closer point which should + never happen. */ + + if (errorPoint != 0) + *errorPoint = p; + + return 0; + } + + p3.x += d; + } + + p3.y += d; + } + + p3.z += d; + } + } + } + } + } + + return 1; +} + +TPE_Vec3 TPE_envLineSegment(TPE_Vec3 point, TPE_Vec3 a, TPE_Vec3 b) +{ + point = TPE_vec3Minus(point,a); + + b = TPE_vec3Minus(b,a); + + point = TPE_vec3Project(point,b); + + if (TPE_vec3Dot(point,b) < 0) + point = TPE_vec3(0,0,0); + else if (TPE_abs(point.x) + TPE_abs(point.y) + TPE_abs(point.z) > + TPE_abs(b.x) + TPE_abs(b.y) + TPE_abs(b.z)) + point = b; + + point = TPE_vec3Plus(point,a); + + return point; +} + +TPE_Vec3 TPE_envHeightmap(TPE_Vec3 point, TPE_Vec3 center, TPE_Unit gridSize, + TPE_Unit (*heightFunction)(int32_t x, int32_t y), TPE_Unit maxDist) +{ + point = TPE_vec3Minus(point,center); + + TPE_Vec3 closestP = TPE_vec3(TPE_INFINITY,TPE_INFINITY,TPE_INFINITY); + TPE_Unit closestD = TPE_INFINITY; + + int16_t startSquareX = point.x / gridSize - (point.x < 0), + startSquareY = point.z / gridSize - (point.z < 0); + + int16_t squareX = startSquareX, + squareY = startSquareY; + + uint8_t spiralDir = 1; + uint16_t spiralStep = 1, spiralStepsLeft = 1; + + TPE_Vec3 // 4 corners of the current square + bl = TPE_vec3(squareX * gridSize,heightFunction(squareX,squareY), + squareY * gridSize), + br = TPE_vec3(bl.x + gridSize,heightFunction(squareX + 1,squareY),bl.z), + tl = TPE_vec3(bl.x,heightFunction(squareX,squareY + 1),bl.z + gridSize), + tr = TPE_vec3(br.x,heightFunction(squareX + 1,squareY + 1),tl.z); + + for (uint16_t i = 0; i < 1024; ++i) // while (1) should work in theory but... + { + if ((TPE_min(TPE_abs(squareX - startSquareX), + TPE_abs(squareY - startSquareY)) - 1) * gridSize + > TPE_min(maxDist,closestD)) + break; // here we can no longer find the dist we're looking for => end + + for (uint8_t j = 0; j < 2; ++j) // check the two triangles of the segment + { + TPE_Vec3 testP = TPE_envHalfPlane(point,j == 0 ? bl : tr, + TPE_vec3Normalized(j == 0 ? + TPE_vec3Cross(TPE_vec3Minus(tl,bl),TPE_vec3Minus(br,bl)) : + TPE_vec3Cross(TPE_vec3Minus(br,tr),TPE_vec3Minus(tl,tr)))); + + TPE_Unit testD = TPE_DISTANCE(testP,point); + + if (testD < closestD) + { + if (j == 0 ? // point is inside the triangle? + (testP.x >= bl.x && testP.z >= bl.z && + (testP.x - bl.x <= tl.z - testP.z)) : + (testP.x <= tr.x && testP.z <= tr.z && + (testP.x - bl.x >= tl.z - testP.z))) + { + closestP = testP; + closestD = testD; + } + else + { + // point outside the triangle, check individual boundary sides +#define testEdge(a,b) \ + testP = TPE_envLineSegment(point,a,b); testD = TPE_DISTANCE(testP,point); \ + if (testD < closestD) { closestP = testP; closestD = testD; } + + testEdge(j == 0 ? bl : tr,br) + testEdge(j == 0 ? bl : tr,tl) + testEdge(br,tl) + +#undef testEdge + } + } + } + + // now step to another square, in spiralling way: + + switch (spiralDir) + { + case 0: // moving up + squareY++; + + bl = tl; br = tr; + tl = TPE_vec3(bl.x,heightFunction(squareX,squareY + 1),bl.z + gridSize); + tr = TPE_vec3(br.x,heightFunction(squareX + 1,squareY + 1),bl.z + + gridSize); + + break; + + case 1: // moving right + squareX++; + + bl = br; tl = tr; + tr = TPE_vec3(tl.x + gridSize,heightFunction(squareX + 1,squareY + 1), + tl.z); + br = TPE_vec3(bl.x + gridSize,heightFunction(squareX + 1,squareY),bl.z); + + break; + + case 2: // moving down + squareY--; + + tl = bl; tr = br; + bl = TPE_vec3(tl.x,heightFunction(squareX,squareY),tl.z - gridSize); + br = TPE_vec3(tr.x,heightFunction(squareX + 1,squareY),tr.z - gridSize); + + break; + + case 3: // moving left + squareX--; + + br = bl; tr = tl; + tl = TPE_vec3(tr.x - gridSize,heightFunction(squareX,squareY + 1),tr.z); + bl = TPE_vec3(br.x - gridSize,heightFunction(squareX,squareY),br.z); + + break; + + default: break; + } + + spiralStepsLeft--; + + if (spiralStepsLeft == 0) + { + spiralDir = spiralDir != 0 ? spiralDir - 1 : 3; + + if (spiralDir == 3 || spiralDir == 1) + spiralStep++; + + spiralStepsLeft = spiralStep; + } + } + + return TPE_vec3Plus(closestP,center); +} + +TPE_Vec3 TPE_envCone(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 direction, + TPE_Unit radius) +{ + point = TPE_vec3Minus(point,center); + + if (TPE_vec3Dot(point,direction) <= 0) + { + // underneath the cone + + direction.x *= -1; + direction.y *= -1; + direction.z *= -1; + + point = TPE_envHalfPlane(point,TPE_vec3(0,0,0),direction); + + TPE_Unit dist = TPE_LENGTH(point); + + if (dist > radius) + { + point.x = (point.x * radius) / dist; + point.y = (point.y * radius) / dist; + point.z = (point.z * radius) / dist; + } + } + else + { + TPE_Unit height = TPE_LENGTH(direction); + + TPE_Vec3 helper = TPE_vec3Project(point,direction); + TPE_Unit y = TPE_LENGTH(helper); + + helper = TPE_vec3Minus(point,helper); + + TPE_Unit x = TPE_LENGTH(helper); + + if (x < 20) + { + // for such small distance big numeric errors occur in the other branch + if (y >= height) + point = direction; + } + else + { + TPE_Unit scaledRadius = radius - ((y * radius) / height); + + if (y > height || x > scaledRadius) // outside? + { + if (x <= 0) + { + TPE_LOG("WARNING: arithmetic error in envCone (library bug)"); + x = 1; // shouldn't happen but just in case, to prevent div by zero + } + + helper.x = (helper.x * radius) / x; + helper.y = (helper.y * radius) / x; + helper.z = (helper.z * radius) / x; + + point = TPE_envLineSegment(point,helper,direction); + } + } + } + + return TPE_vec3Plus(point,center); +} + +static inline uint8_t TPE_bodyIsActive(const TPE_Body *body) +{ + return !(body->flags & TPE_BODY_FLAG_DEACTIVATED); +} + +#endif // guard