Licar/tinyphysicsengine.h

#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 <stdint.h>

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. */
#define TPE_BODY_FLAG_NO_BSPHERE 64    /**< Stops quick bounding sphere checks
                                            against environment. */
#define TPE_BODY_FLAG_UNRESOLVED 128   /**< Set automatically for bodies whose
                                            collision with environment failed
                                            to be resolved. */

/** 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, TPE_Unit offset);

#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 triangular 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;

  body->flags &= ~TPE_BODY_FLAG_UNRESOLVED;

  if (!(body->flags & TPE_BODY_FLAG_NO_BSPHERE))
  {
    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 (r == 2)
        body->flags |= TPE_BODY_FLAG_UNRESOLVED;

      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, TPE_Unit offset)
{
#define Z_LIMIT 250
  if (world->environmentFunction != 0)
  {
    // environment:

    TPE_Vec3 testPoint;

    TPE_Unit gridHalfSize = (envGridSize * envGridRes) / 2;

    TPE_Vec3 center;

    offset %= envGridSize;

    if (envGridRes != 0)
    {
      center = TPE_vec3(0,TPE_sin(camRot.x),TPE_cos(camRot.x));

      _TPE_vec2Rotate(&center.x,&center.z,camRot.y);

      center = TPE_vec3Times(center,gridHalfSize);
      center = TPE_vec3Plus(camPos,center);

      center.x = (center.x / envGridSize) * envGridSize + offset;
      center.y = (center.y / envGridSize) * envGridSize + offset;
      center.z = (center.z / envGridSize) * envGridSize + offset;
    }

    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