Licar/racing.h

#ifndef _LCR_RACING_H
#define _LCR_RACING_H

/**
  Racing module: implements the racing physics and logic as well as replays and
  other related things.

  Some comments:
  - Replays are internally stored as follows: the replay consists of 16 bit
    words representing changes in input at specific frame. In lowest 4 bits the
    new input state is recorded, the remaining 12 bits record physics frame
    offset against the previous input change. If the 12 bits don't suffice
    because the offset is too big (input didn't change for more than 2^12
    frames), there must simply be inserted an extra word that just copies the
    current input state.
  - Replay text format: first there is the name of the map terminated by ';',
    then hexadecimal hash of the map follows (exactly 8 characters), then
    blank character follows, then achieved time as a series of decimal digits
    expressing the number of milliseconds, then the replay data, i.e. the series
    of 16 bit words in hexadecimal, each preceded by ':'. The events (but
    nothing else) may otherwise be preceeded or followed by other characters
    (possible comments). All hexadecimal letters must be lowercase. The word
    00000000 may optinally be used to terminate the replay, the rest of the
    string will be ignored.
*/

typedef int32_t LCR_GameUnit;  ///< abstract game unit

#define LCR_GAME_UNIT 1024     ///< length of map square in LCR_GameUnits

#define LCR_RACING_INPUT_FORW        0x01
#define LCR_RACING_INPUT_RIGHT       0x02
#define LCR_RACING_INPUT_BACK        0x04
#define LCR_RACING_INPUT_LEFT        0x08

#define LCR_RACING_EVENT_CP_TAKEN    0x0001
#define LCR_RACING_EVENT_FINISHED    0x0002
#define LCR_RACING_EVENT_CRASH_SMALL 0x0004
#define LCR_RACING_EVENT_CRASH_BIG   0x0008

#define LCR_PHYSICS_UNIT             4096 ///< len. of square for phys. engine
  
#define TPE_RESHAPE_TENSION_LIMIT 3
#define TPE_RESHAPE_ITERATIONS    8

#include "general.h"
#include "map.h"
#include "tinyphysicsengine.h"

// TODO: move some of this to constants?

#define LCR_GRAVITY (LCR_PHYSICS_UNIT / 160)
#define LCR_FAN_FORCE 3
#define LCR_FAN_FORCE_DECREASE 6

#define LCR_CAR_FORWARD_FRICTION (TPE_F / 180)
#define LCR_CAR_AIR_FRICTION 32
#define LCR_CAR_STAND_FRICTION_MULTIPLIER 32
#define LCR_CAR_STEER_FRICTION (TPE_F)
#define LCR_CAR_ELASTICITY (TPE_F / 150)
#define LCR_CAR_ACCELERATION (LCR_PHYSICS_UNIT / 19)
#define LCR_CAR_STEER_SPEED (LCR_GAME_UNIT / 17)
#define LCR_CAR_STEER_MAX (LCR_GAME_UNIT / 2)

#define LCR_CAR_DRIFT_THRESHOLD_1 (LCR_GAME_UNIT / 4)
#define LCR_CAR_DRIFT_THRESHOLD_0 (LCR_GAME_UNIT / 200)

#define LCR_CAR_CRASH_SPEED_DIFF 25
#define LCR_CAR_CRASH_SPEED_THRESHOLD 70

// multipliers (in 8ths) of friction and acceleration on concrete:
#define LCR_CAR_GRASS_FACTOR 5
#define LCR_CAR_DIRT_FACTOR 3
#define LCR_CAR_ICE_FACTOR 1
#define LCR_CAR_DRIFT_FACTOR 2    ///< only affects steering friction

#define LCR_CAR_JOINTS 5
#define LCR_CAR_CONNECTIONS 10

#define LCR_REPLAY_EVENT_END 0xff ///< special event fed to replay at the end

struct
{
  TPE_World physicsWorld;
  TPE_Body carBody;
  TPE_Joint carJoints[LCR_CAR_JOINTS];
  TPE_Connection carConnections[LCR_CAR_CONNECTIONS];

  uint32_t tick;
  uint8_t wheelCollisions;   /**< In individual bits records for each car wheel
                                  whether it's currently touching the ground.
                                  Lower bits record current collisions, higher
                                  bits the previous state (for averaging). */

  TPE_Vec3 carPositions[2];  ///< Current and previous position in game units.
  TPE_Vec3 carRotations[2];  ///< Current and previous rotation in game units.
  uint8_t carDrifting;       ///< Whether or not the car is currently in drift.

  TPE_Unit fanForce;         ///< Upwards acceleration caused by a fan.
  LCR_GameUnit wheelAngle;   ///< Current wheel angle, 0 to LCR_GAME_UNIT.
  LCR_GameUnit wheelSteer;   ///< Left/right steer, LCR_CAR_STEER_MAX bounds.

  LCR_GameUnit carSpeeds[2]; /**< Signed speed in game units per tick (negative
                                  if backwards) and its previous value. */

  // for fixing physics bugs:
  TPE_Vec3 carOKPositions[LCR_CAR_JOINTS];
  uint8_t carNotOKCount;

  uint8_t playingReplay;
} LCR_racing;

struct
{
  uint16_t eventCount;
  uint16_t events[LCR_SETTING_REPLAY_MAX_SIZE]; 

  // for playing
  uint16_t currentEvent;
  uint16_t currentFrame;
  uint32_t achievedTime;
} LCR_replay;

/**
  Gets times of the run in milliseconds.
*/
uint32_t LCR_racingGetRunTimeMS()
{
  return LCR_racing.tick * LCR_RACING_TICK_MS;
}

TPE_Vec3 _LCR_TPE_vec3DividePlain(TPE_Vec3 v, TPE_Unit d)
{
  v.x /= d; v.y /= d; v.z /= d;
  return v;
}

/**
  Initializes replay for recording.
*/
void LCR_replayInitRecording(void)
{
  LCR_LOG1("initializing replay recording");
  LCR_replay.eventCount = 0;
  LCR_replay.achievedTime = 0;
}

void LCR_replayInitPlaying(void)
{
  LCR_LOG1("initializing replay playing");
  LCR_replay.currentEvent = 0;
  LCR_replay.currentFrame = 0;
}

/**
  Outputs current replay using provided character printing function. The string
  will be zero terminated.
*/
void LCR_replayOutputStr(void (*printChar)(char))
{
  LCR_LOG1("outputting replay");

  const char *s = LCR_currentMap.name;

  while (*s)
  {
    printChar(*s);
    s++;
  }

  printChar(';');

  uint32_t hash = LCR_currentMap.hash;

  for (int i = 0; i < 8; ++i)
  {
    printChar(_LCR_hexDigit(hash % 16));
    hash /= 16;
  }

  printChar(' ');

  // 8 decimal digits are enough to record 24 hours

#define PUTD(order) printChar('0' + (LCR_replay.achievedTime / order) % 10);
  PUTD(10000000) PUTD(1000000) PUTD(100000) PUTD(10000)
  PUTD(1000) PUTD(100) PUTD(10) PUTD(1)
#undef PUTD

  for (int i = 0; i < LCR_replay.eventCount; ++i)
  {
    uint16_t e = LCR_replay.events[i];
    printChar(':');

    for (int j = 0; j < 4; ++j)
    {
      printChar(_LCR_hexDigit((e >> 12) & 0x0f));
      e <<= 4;
    }
  }

  printChar('\n');
}

/**
  Reads replay from string using provided function that returns next character
  in the string. Returns 1 on success, else 0.
*/
int LCR_replayLoadFromStr(char (*nextChar)(void))
{
  char c = ' ';

  LCR_replay.eventCount = 0;
  LCR_replay.achievedTime = 0;

  do // map name
  {
    c = nextChar();

    if (c == 0)
      return 0;
  } while (c != ';');

  for (int i = 0; i < 8; ++i) // hash
    if (_LCR_hexDigitVal(nextChar()) < 0)
      return 0;

  nextChar();

  while (1) // time
  {
    c = nextChar();

    if (c < '0' || c > '9')
      break;

    LCR_replay.achievedTime = LCR_replay.achievedTime * 10 + c - '0';
  }

  while (c != 0) // events
  {
    if (c == ':')
    {
      uint16_t e = 0;

      for (int i = 0; i < 4; ++i)
        e = (e << 4) | _LCR_hexDigitVal(nextChar());

      if (e == 0)
        break;
      
      if (LCR_replay.eventCount >= LCR_SETTING_REPLAY_MAX_SIZE)
        return 0;

      LCR_replay.events[LCR_replay.eventCount] = e;
      LCR_replay.eventCount++;
    }

    c = nextChar();
  }

  return 1;
}

/**
  During playing of a replay returns the next input and shifts to next frame.
*/
uint8_t LCR_replayGetNextInput(void)
{
  if (LCR_replay.currentEvent >= LCR_replay.eventCount)
    return 0;

  if (LCR_replay.currentFrame ==
    (LCR_replay.events[LCR_replay.currentEvent] >> 4))
  {
    LCR_replay.currentEvent++;
    LCR_replay.currentFrame = 0;
  }
 
  LCR_replay.currentFrame++;

  return LCR_replay.currentEvent ?
    (LCR_replay.events[LCR_replay.currentEvent - 1] & 0x0f) : 0;
}

int LCR_replayHasFinished(void)
{
  return LCR_replay.currentEvent >= LCR_replay.eventCount;
}

/**
  Records another input event. Returns 1 on success, or 0 if the event couldn't
  be recorded. The event is only added if necessary, i.e. this function can
  (and must) be called every frame without worrying about inflating its size.
  When the run ends, the LCR_REPLAY_EVENT_END has to be fed!
*/
int LCR_replayRecordEvent(uint32_t frame, uint8_t input)
{
  LCR_LOG2("recording replay event");

  if (LCR_replay.achievedTime)
    return 1; // already finished

  if (input == LCR_REPLAY_EVENT_END)
  {
    LCR_replay.achievedTime = frame;
    LCR_LOG1("replay recording finished");
    return 1;
  }

#if LCR_SETTING_REPLAY_MAX_SIZE > 0
  uint8_t previousInput = 0;
  uint32_t previousFrame = 0;

  for (int i = 0; i < LCR_replay.eventCount; ++i)
  {
    previousInput = LCR_replay.events[i] & 0x0f;
    previousFrame += LCR_replay.events[i] >> 4;
  }

  if (input == previousInput)
    return 1;

  if (frame < previousFrame)
    return 0;

  frame -= previousFrame; // convert to offset

  while (frame > 4095 && LCR_replay.eventCount < LCR_SETTING_REPLAY_MAX_SIZE)
  {
    // add intermediate events
    frame -= 4095;
    previousFrame += 4095;

    LCR_replay.events[LCR_replay.eventCount] =
      (previousFrame << 4) | previousInput;
    LCR_replay.eventCount++;
  }

  if (LCR_replay.eventCount >= LCR_SETTING_REPLAY_MAX_SIZE)
    return 0;

  LCR_replay.events[LCR_replay.eventCount] = (frame << 4) | (input & 0x0f);
  LCR_replay.eventCount++;
#endif

  return 1;
}

/**
  Helper function for _LCR_racingEnvironmentFunction, returns closest point
  on a map block placed at coordinate origin.
*/
TPE_Vec3 _LCR_racingBlockEnvFunc(TPE_Vec3 point, const uint8_t *block)
{
  TPE_Vec3 v, vBest;
  TPE_Unit d, dBest = TPE_INFINITY;

  uint8_t bx, by, bz;
  LCR_mapBlockGetCoords(block,&bx,&by,&bz);
 
  TPE_Vec3 blockOffset = TPE_vec3(
    (((int) bx) - LCR_MAP_SIZE_BLOCKS / 2) * LCR_PHYSICS_UNIT,
    (((int) by) - LCR_MAP_SIZE_BLOCKS / 2) * (LCR_PHYSICS_UNIT / 2),
    (((int) bz) - LCR_MAP_SIZE_BLOCKS / 2) * LCR_PHYSICS_UNIT);

  point = TPE_vec3Minus(point,blockOffset); // shift to origin

  uint8_t transform = 
    LCR_mapBlockOppositeTransform(LCR_mapBlockGetTransform(block));

  LCR_TRANSFORM_COORDS(transform,point.x,point.y,point.z,LCR_PHYSICS_UNIT,
    (LCR_PHYSICS_UNIT / 2))

  point = TPE_vec3Minus(point,
    TPE_vec3(LCR_PHYSICS_UNIT / 2,LCR_PHYSICS_UNIT / 4,LCR_PHYSICS_UNIT / 2));

  switch (block[0])
  {
    case LCR_BLOCK_FULL:
    case LCR_BLOCK_BOTTOM:
    case LCR_BLOCK_BOTTOM_ACCEL:
    case LCR_BLOCK_LEFT:
    case LCR_BLOCK_BOTTOM_LEFT:
    case LCR_BLOCK_BOTTOM_LEFT_FRONT:
    case LCR_BLOCK_FULL_ACCEL:
    case LCR_BLOCK_FULL_FAN:
    {
      TPE_Vec3
        offset = TPE_vec3(0,0,0),
        size = TPE_vec3(LCR_PHYSICS_UNIT / 2,LCR_PHYSICS_UNIT / 4,
          LCR_PHYSICS_UNIT / 2);

      if (block[0] == LCR_BLOCK_BOTTOM ||
          block[0] == LCR_BLOCK_BOTTOM_ACCEL ||
          block[0] == LCR_BLOCK_BOTTOM_LEFT ||
          block[0] == LCR_BLOCK_BOTTOM_LEFT_FRONT)
      {
        offset.y -= LCR_PHYSICS_UNIT / 8;
        size.y = LCR_PHYSICS_UNIT / 8;
      }

      if (block[0] == LCR_BLOCK_LEFT ||
          block[0] == LCR_BLOCK_BOTTOM_LEFT ||
          block[0] == LCR_BLOCK_BOTTOM_LEFT_FRONT)
      {
        offset.x -= LCR_PHYSICS_UNIT / 4;
        size.x = LCR_PHYSICS_UNIT / 4;
      }

      if (block[0] == LCR_BLOCK_BOTTOM_LEFT_FRONT)
      {
        offset.z -= LCR_PHYSICS_UNIT / 4;
        size.z = LCR_PHYSICS_UNIT / 4;
      }

      point = TPE_envAABox(point,offset,size);
      break;
    }

#define _CHECK_NEXT(check)\
  v = check;\
  d = TPE_dist(point,v);\
  if (d < dBest) {\
    vBest = v;\
    dBest = d;}\
  if (dBest == 0) {\
    point = vBest;\
    break;}

    case LCR_BLOCK_RAMP_CURVED_WALL:
      _CHECK_NEXT(TPE_envAABox(point,TPE_vec3(5 * LCR_PHYSICS_UNIT / 12,0,0),
        TPE_vec3(LCR_PHYSICS_UNIT / 12,LCR_PHYSICS_UNIT / 4,LCR_PHYSICS_UNIT
        / 2)));
      // fall through

    case LCR_BLOCK_RAMP_CURVED_PLAT:
      _CHECK_NEXT(TPE_envAABox(point,TPE_vec3(0,0,5 * LCR_PHYSICS_UNIT / 12),
      TPE_vec3(LCR_PHYSICS_UNIT / 2,LCR_PHYSICS_UNIT / 4,LCR_PHYSICS_UNIT / 12
      )));
      // fall through

    case LCR_BLOCK_RAMP_CURVED:
    {
      TPE_Unit sides[6];
      TPE_Unit rampShift = block[0] != LCR_BLOCK_RAMP_CURVED ?
        LCR_PHYSICS_UNIT / 6 : 0;

      sides[0] = -1 * LCR_PHYSICS_UNIT / 8 - rampShift;
      sides[1] = -1 * LCR_PHYSICS_UNIT / 4;
      sides[2] = LCR_PHYSICS_UNIT / 2 - rampShift;
      sides[3] = -1 * LCR_PHYSICS_UNIT / 4;
      sides[4] = LCR_PHYSICS_UNIT / 2 - rampShift;
      sides[5] = LCR_PHYSICS_UNIT / 4;

      _CHECK_NEXT(TPE_envAATriPrism(point,
        TPE_vec3(0,0,0),sides,LCR_PHYSICS_UNIT,2))

      sides[0] = -1 * LCR_PHYSICS_UNIT / 2;
      sides[1] = -1 * LCR_PHYSICS_UNIT / 4;
      sides[2] = LCR_PHYSICS_UNIT / 2 - rampShift;
      sides[3] = -1 * LCR_PHYSICS_UNIT / 4;
      sides[4] = LCR_PHYSICS_UNIT / 2 - rampShift;
      sides[5] = LCR_PHYSICS_UNIT / 8;

      _CHECK_NEXT(TPE_envAATriPrism(point,TPE_vec3(0,0,0),sides,LCR_PHYSICS_UNIT
        ,2));

      point = vBest;

      break;
    }

    case LCR_BLOCK_CORNER_CONVEX:
    case LCR_BLOCK_CORNER_CONCAVE:
    {
      TPE_Unit sides[6];
      sides[0] = -1 * LCR_PHYSICS_UNIT / 2;
      sides[1] = LCR_PHYSICS_UNIT / 2;
      sides[2] = -1 * LCR_PHYSICS_UNIT / 2;
      sides[3] = -1 * LCR_PHYSICS_UNIT / 2;
      sides[4] = LCR_PHYSICS_UNIT / 5;
      sides[5] = -1 * LCR_PHYSICS_UNIT / 5;

      if (block[0] == LCR_BLOCK_CORNER_CONCAVE)
      {
        sides[4] *= -1;
        sides[5] *= -1; 
      }

      _CHECK_NEXT(TPE_envAATriPrism(point,TPE_vec3(0,0,0),sides,
        LCR_PHYSICS_UNIT / 2,1));

      sides[2] = sides[4];
      sides[3] = sides[5];

      sides[4] = LCR_PHYSICS_UNIT / 2;
      sides[5] = LCR_PHYSICS_UNIT / 2;

      _CHECK_NEXT(TPE_envAATriPrism(point,TPE_vec3(0,0,0),sides,
        LCR_PHYSICS_UNIT / 2,1));

      point = vBest;

      break;
    }

#undef _CHECK_NEXT

    case LCR_BLOCK_RAMP_CORNER:
    {
      TPE_Unit sides[6];

      sides[0] = -1 * LCR_PHYSICS_UNIT / 2;
      sides[1] = -1 * LCR_PHYSICS_UNIT / 4;
      sides[2] = LCR_PHYSICS_UNIT / 2;
      sides[3] = -1 * LCR_PHYSICS_UNIT / 4;
      sides[4] = LCR_PHYSICS_UNIT / 2;
      sides[5] = LCR_PHYSICS_UNIT / 4;

      if (point.x > LCR_PHYSICS_UNIT / 2)
      {
        sides[4] *= -1;

        point = TPE_envAATriPrism(point,TPE_vec3(0,0,0),
          sides,LCR_PHYSICS_UNIT,2);
      }
      else if (point.z < -1 * LCR_PHYSICS_UNIT / 2)
        point = TPE_envAATriPrism(point,TPE_vec3(0,0,0),
          sides,LCR_PHYSICS_UNIT,0);
      else if (point.y < -1 * LCR_PHYSICS_UNIT / 4)
      {
        sides[1] *= 2;
        sides[3] *= 2;
        sides[5] *= 2;

        point = TPE_envAATriPrism(point,
          TPE_vec3(0,0,0),sides,LCR_PHYSICS_UNIT / 2,1);
      }
      else
      {
        point = TPE_envHalfPlane(point,TPE_vec3(LCR_PHYSICS_UNIT / 2,
          LCR_PHYSICS_UNIT / 4,-1 * LCR_PHYSICS_UNIT / 2),
          TPE_vec3(-1 * LCR_PHYSICS_UNIT,2 * LCR_PHYSICS_UNIT,LCR_PHYSICS_UNIT));

#define LINESNAP(a,b,c,d,e,f)\
  point = TPE_envLineSegment(point,\
    TPE_vec3(a * LCR_PHYSICS_UNIT / 2,b * LCR_PHYSICS_UNIT / 4,\
    c * LCR_PHYSICS_UNIT / 2),TPE_vec3(d * LCR_PHYSICS_UNIT / 2,\
    e * LCR_PHYSICS_UNIT / 4,f * LCR_PHYSICS_UNIT / 2));

        if (point.y < -1 * LCR_PHYSICS_UNIT / 4)
          LINESNAP(-1,-1,-1,1,-1,1)
        else if (point.x > LCR_PHYSICS_UNIT / 2)
          LINESNAP(1,1,-1,1,-1,1)
        else if (point.z < -1 * LCR_PHYSICS_UNIT / 2)
          LINESNAP(-1,-1,-1,1,1,-1)
#undef LINESNAP
      }

      break;
    }

    case LCR_BLOCK_RAMP:
    case LCR_BLOCK_RAMP_34:
    case LCR_BLOCK_RAMP_12:
    case LCR_BLOCK_RAMP_14:
    case LCR_BLOCK_RAMP_STEEP:
    case LCR_BLOCK_RAMP_ACCEL:
    case LCR_BLOCK_RAMP_FAN:
    {
      uint8_t front, top;
      LCR_rampGetDimensions(block[0],&top,&front);
      front = 6 - front;

      TPE_Unit sides[6];
      sides[0] = 
        -1 * LCR_PHYSICS_UNIT / 2 + (LCR_PHYSICS_UNIT / 6) * ((int) front);

      sides[1] = -1 * LCR_PHYSICS_UNIT / 4;

      sides[2] = LCR_PHYSICS_UNIT / 2;
      sides[3] = -1 * LCR_PHYSICS_UNIT / 4;

      sides[4] = LCR_PHYSICS_UNIT / 2;
      sides[5] = -1 * LCR_PHYSICS_UNIT / 4 +
        ((int) top) * (LCR_PHYSICS_UNIT / 8);

      point = TPE_envAATriPrism(point,TPE_vec3(0,0,0),sides,LCR_PHYSICS_UNIT,2);
      break;
    }

    case LCR_BLOCK_HILL:
    {
      point = (point.y > -1 * LCR_PHYSICS_UNIT / 4 && point.z
        < LCR_PHYSICS_UNIT / 4) ?
          TPE_envCylinder(point,
            TPE_vec3(0,-1 * LCR_PHYSICS_UNIT / 2,LCR_PHYSICS_UNIT / 4),
            TPE_vec3(LCR_PHYSICS_UNIT / 2,0,0),
            LCR_PHYSICS_UNIT / 2 + LCR_PHYSICS_UNIT / 4) :
          TPE_envAABox(point,TPE_vec3(0,0,0),
            TPE_vec3(LCR_PHYSICS_UNIT / 2,LCR_PHYSICS_UNIT / 4,
            LCR_PHYSICS_UNIT / 2));

      if (point.y < -1 * LCR_PHYSICS_UNIT / 4) // for some reason happens somet.
        point.y = -1 * LCR_PHYSICS_UNIT / 4;

      break;
    }

    case LCR_BLOCK_BUMP:
      point = TPE_envCone(point,TPE_vec3(0,-1 * LCR_PHYSICS_UNIT / 4 ,0),
        TPE_vec3(0,LCR_PHYSICS_UNIT / 6,0),(5 * LCR_PHYSICS_UNIT) / 12);
      break;

    case LCR_BLOCK_CORNER:
    case LCR_BLOCK_CORNER_12:
    {
      TPE_Unit sides[6];
      sides[0] = -1 * LCR_PHYSICS_UNIT / 2;
      sides[1] = LCR_PHYSICS_UNIT / 2;
      sides[2] = -1 * LCR_PHYSICS_UNIT / 2;
      sides[3] = -1 * LCR_PHYSICS_UNIT / 2;
      sides[4] = block[0] == LCR_BLOCK_CORNER ? LCR_PHYSICS_UNIT / 2 : 0;
      sides[5] = LCR_PHYSICS_UNIT / 2;

      point = TPE_envAATriPrism(point,TPE_vec3(0,0,0),sides,
        LCR_PHYSICS_UNIT / 2,1);
      
      break;
    }

    default:
      point = TPE_vec3(0,0,LCR_MAP_SIZE_BLOCKS * LCR_PHYSICS_UNIT);
      break;
  }

  point = TPE_vec3Plus(point,
    TPE_vec3(LCR_PHYSICS_UNIT / 2,LCR_PHYSICS_UNIT / 4,LCR_PHYSICS_UNIT / 2));

  transform = LCR_mapBlockOppositeTransform(transform);

  LCR_TRANSFORM_COORDS(transform,point.x,point.y,point.z,LCR_PHYSICS_UNIT,
    (LCR_PHYSICS_UNIT / 2))

  point = TPE_vec3Plus(point,blockOffset); // shift back

  return point;
}

/**
  For tinyphysicsengine, function that defines the shape of physics world,
  returns closest point to any given point in space.
*/
TPE_Vec3 _LCR_racingEnvironmentFunction(TPE_Vec3 point, TPE_Unit maxDist)
{
  // start with the map outside walls:
  TPE_ENV_START(TPE_envAABoxInside(point,TPE_vec3(0,0,0),TPE_vec3(
    LCR_PHYSICS_UNIT * LCR_MAP_SIZE_BLOCKS,
    (LCR_PHYSICS_UNIT * LCR_MAP_SIZE_BLOCKS) / 2,
    LCR_PHYSICS_UNIT * LCR_MAP_SIZE_BLOCKS)),point)

  // without this check we might try to get block outside the map 
  if (_pBest.x == point.x && _pBest.y == point.y && _pBest.z == point.z)
    return _pBest;

  if (maxDist <= LCR_PHYSICS_UNIT / 4) // considering half of square height
  {
    /* Here we only check the 8 closest blocks => relatively fast. */

    TPE_Vec3 pointShifted = TPE_vec3Plus(point,TPE_vec3(
        (LCR_MAP_SIZE_BLOCKS / 2) * LCR_PHYSICS_UNIT,
        (LCR_MAP_SIZE_BLOCKS / 4) * LCR_PHYSICS_UNIT,
        (LCR_MAP_SIZE_BLOCKS / 2) * LCR_PHYSICS_UNIT));

    uint8_t coords[6]; // x_low, x_high, y_low, y_high, z_low, z_high

    coords[0] = (pointShifted.x / LCR_PHYSICS_UNIT);
    coords[1] = (pointShifted.x % LCR_PHYSICS_UNIT < LCR_PHYSICS_UNIT / 2);

    coords[2] = (pointShifted.y / (LCR_PHYSICS_UNIT / 2));
    coords[3] =
      (pointShifted.y % (LCR_PHYSICS_UNIT / 2) < LCR_PHYSICS_UNIT / 4);

    coords[4] = (pointShifted.z / LCR_PHYSICS_UNIT);
    coords[5] = (pointShifted.z % LCR_PHYSICS_UNIT < LCR_PHYSICS_UNIT / 2);

    for (int i = 0; i < 6; i += 2)
      if (coords[i + 1])
      {
        coords[i + 1] = coords[i];
        coords[i] = coords[i] > 0 ? coords[i] - 1 : 0;
      }
      else
        coords[i + 1] = coords[i] < 63 ? coords[i] + 1 : 63;

    int start = 0, end = LCR_currentMap.blockCount - 1;

    for (uint8_t i = 0; i < 8; ++i)
    {
      /* Black magic: here we make it so that we the lowest coord numbers
        (0,0,0), then the highest (1,1,1), then second lowest (1,0,0), then
        second highest (0,1,1) etc. This way we are narrowing the range (start,
        end) for the binary search. */

      int blockNum = LCR_mapGetBlockAtFast(
        coords[0] + ((i ^ (i >> 1)) & 0x01),
        coords[2] + ((i ^ (i >> 2)) & 0x01),
        coords[4] + (i & 0x01),start,end);

      if (blockNum >= 0) // is there a block at the coords?
      {
        TPE_ENV_NEXT(_LCR_racingBlockEnvFunc(point, // check it
          LCR_currentMap.blocks + blockNum * LCR_BLOCK_SIZE),point)

        // narrow the search range:
        if (i % 2 == 0 && blockNum > start)
          start = blockNum;

        if (i % 2 && blockNum < end)
          end = blockNum;
      }
    }
  }
  else
  {
    LCR_LOG1("collision checking all blocks (shouldn't happen often!)");

    const uint8_t *block = LCR_currentMap.blocks;

    // Full check of all map blocks, slow, shouldn't happen often!
    for (int j = 0; j < LCR_currentMap.blockCount; ++j)
    {
      TPE_ENV_NEXT(_LCR_racingBlockEnvFunc(point,block),point)
      block += LCR_BLOCK_SIZE;
    }
  }

  TPE_ENV_END
}

LCR_GameUnit LCR_racingGetCarSpeedUnsigned(void)
{
  return LCR_racing.carSpeeds[0] >= 0 ? LCR_racing.carSpeeds[0] :
    (-1 * LCR_racing.carSpeeds[0]);
}

LCR_GameUnit LCR_racingGetCarSpeedSigned(void)
{
  return LCR_racing.carSpeeds[0];
}

uint8_t _LCR_racingCollisionHandler(uint16_t b1, uint16_t j1, uint16_t b2,
  uint16_t j2, TPE_Vec3 p)
{
  // check which wheels are touching the ground.

  if (j1 < 4) // wheel joint?
    LCR_racing.wheelCollisions |= 0x01 << j1;

  return 1;
}

LCR_GameUnit _LCR_racingSmoothRot(LCR_GameUnit angleNew, LCR_GameUnit angleOld)
{
  /* We'll smooth small rotations by averaging the last two angles; bigger
     rotations won't be smoothed -- firstly this removes lag for fast rotations
     and also deals with the issue of averaging e.g. 1 and 359 degrees. */

  LCR_GameUnit diff = angleNew - angleOld;

  if (diff > LCR_GAME_UNIT / 8 || diff < -1 * LCR_GAME_UNIT / 8)
    return angleNew;

  return angleOld + diff / 2;
}

TPE_Vec3 _LCR_racingGetWheelCenterPoint(void)
{
  return _LCR_TPE_vec3DividePlain(
    TPE_vec3Plus(
      TPE_vec3Plus(
        LCR_racing.carBody.joints[0].position,
        LCR_racing.carBody.joints[1].position),
      TPE_vec3Plus(
        LCR_racing.carBody.joints[2].position,
        LCR_racing.carBody.joints[3].position)),4);
}

void _LCR_racingUpdateCarPosRot(void)
{
  TPE_Vec3 tmpVec = LCR_racing.carPositions[0];

  LCR_racing.carPositions[0] = _LCR_TPE_vec3DividePlain(
    TPE_vec3TimesPlain(_LCR_racingGetWheelCenterPoint(),LCR_GAME_UNIT),
      LCR_PHYSICS_UNIT);

  LCR_racing.carPositions[0] = // smooth the position
    TPE_vec3KeepWithinBox(LCR_racing.carPositions[1],LCR_racing.carPositions[0],
        TPE_vec3(
          LCR_PHYSICS_UNIT / 64, // TODO: constant
          LCR_PHYSICS_UNIT / 64,
          LCR_PHYSICS_UNIT / 64));

  LCR_racing.carPositions[1] = tmpVec;

  tmpVec = _LCR_TPE_vec3DividePlain(TPE_vec3TimesPlain(TPE_bodyGetRotation(
    &(LCR_racing.carBody),0,2,1),LCR_GAME_UNIT),TPE_F);

  LCR_racing.carRotations[1] = LCR_racing.carRotations[0];
  LCR_racing.carRotations[0] = tmpVec;
}

/**
  Initializes new run.
*/
void LCR_racingRestart(uint8_t replay)
{
  LCR_LOG0("restarting race");
  LCR_mapReset();

  LCR_racing.tick = 0;
  LCR_racing.fanForce = 0;

  LCR_racing.playingReplay = replay;

  TPE_bodyActivate(&(LCR_racing.carBody));
  LCR_racing.wheelCollisions = 0;

  LCR_racing.wheelAngle = 0;
  LCR_racing.wheelSteer = 0;
  LCR_racing.carSpeeds[0] = 0;
  LCR_racing.carSpeeds[1] = 0;
  LCR_racing.carDrifting = 0;

  // make the car body:
  TPE_makeCenterRectFull(LCR_racing.carJoints,
    LCR_racing.carConnections,
    LCR_PHYSICS_UNIT / 2,
    (LCR_PHYSICS_UNIT * 3) / 4,
    LCR_PHYSICS_UNIT / 8);

  LCR_racing.carJoints[4].position.y += LCR_PHYSICS_UNIT / 6;
  LCR_racing.carJoints[4].sizeDivided *= 3;
  LCR_racing.carJoints[4].sizeDivided /= 2;

  TPE_bodyInit(&(LCR_racing.carBody),LCR_racing.carJoints,LCR_CAR_JOINTS,
    LCR_racing.carConnections,LCR_CAR_CONNECTIONS,TPE_F);

  LCR_racing.carBody.friction = LCR_CAR_FORWARD_FRICTION;
  LCR_racing.carBody.elasticity = LCR_CAR_ELASTICITY;
  LCR_racing.carBody.flags |= TPE_BODY_FLAG_ALWAYS_ACTIVE;

  /* We disable bounding sphere checks because that would lead to calling env.
    function with large min. distance which would lead to slow iteration over
    all map blocks. */
  LCR_racing.carBody.flags |= TPE_BODY_FLAG_NO_BSPHERE;

  TPE_bodyMoveTo(&(LCR_racing.carBody),TPE_vec3(
    (((TPE_Unit) LCR_currentMap.startPos[0]) - LCR_MAP_SIZE_BLOCKS / 2)
      * LCR_PHYSICS_UNIT + LCR_PHYSICS_UNIT / 2,
    (((TPE_Unit) LCR_currentMap.startPos[1]) - LCR_MAP_SIZE_BLOCKS / 2)
      * LCR_PHYSICS_UNIT / 2 + LCR_PHYSICS_UNIT / 4,
    (((TPE_Unit) LCR_currentMap.startPos[2]) - LCR_MAP_SIZE_BLOCKS / 2)
      * LCR_PHYSICS_UNIT + LCR_PHYSICS_UNIT / 2));

  if (LCR_currentMap.startPos[3])
  {
    uint8_t trans = LCR_currentMap.startPos[3];

    if (trans & LCR_BLOCK_TRANSFORM_FLIP_V)
      TPE_bodyRotateByAxis(&(LCR_racing.carBody),
        TPE_vec3(TPE_FRACTIONS_PER_UNIT / 2,0,0));

    trans &=
      LCR_BLOCK_TRANSFORM_ROT_90 |
      LCR_BLOCK_TRANSFORM_ROT_180 |
      LCR_BLOCK_TRANSFORM_ROT_270;

    TPE_bodyRotateByAxis(&(LCR_racing.carBody),
      TPE_vec3(0,(trans == LCR_BLOCK_TRANSFORM_ROT_90) ?
        3 * TPE_F / 4 : (trans == LCR_BLOCK_TRANSFORM_ROT_180) ?
          TPE_F / 2 : (TPE_F / 4),0));
  }

  for (int i = 0; i < LCR_CAR_JOINTS; ++i)
    LCR_racing.carOKPositions[i] = TPE_vec3(0,0,0);

  LCR_racing.carNotOKCount = 0;

  LCR_racing.carPositions[0] = TPE_vec3(0,0,0);
  LCR_racing.carPositions[1] = LCR_racing.carPositions[0];
  LCR_racing.carRotations[0] = TPE_vec3(0,0,0);
  LCR_racing.carRotations[1] = LCR_racing.carRotations[0];

  _LCR_racingUpdateCarPosRot();

  LCR_racing.carPositions[1] = LCR_racing.carPositions[0];
  LCR_racing.carRotations[1] = LCR_racing.carRotations[0];
}

/**
  Initializes the racing module, only call once.
*/
void LCR_racingInit(void)
{
  LCR_LOG0("initializing racing engine");

  TPE_worldInit(&(LCR_racing.physicsWorld),
  &(LCR_racing.carBody),1,_LCR_racingEnvironmentFunction);

  LCR_racing.playingReplay = 0;
  LCR_racing.physicsWorld.collisionCallback = _LCR_racingCollisionHandler;
}

/**
  Gets current car transformation intended for rendering, i.e. potentially with
  smoothing and interpolation applied to the underlying internal state in the
  physics engine.
*/
void LCR_racingGetCarTransform(LCR_GameUnit position[3],
  LCR_GameUnit rotation[3], LCR_GameUnit interpolationParam)
{
  LCR_LOG2("getting car transform");  

  TPE_Vec3 v;

#if LCR_SETTING_SMOOTH_ANIMATIONS
  v = TPE_vec3Plus(LCR_racing.carPositions[1], // LERP previous and current pos
    _LCR_TPE_vec3DividePlain(
      TPE_vec3TimesPlain(TPE_vec3Minus(
        LCR_racing.carPositions[0],LCR_racing.carPositions[1]),
          interpolationParam),LCR_GAME_UNIT));

  position[0] = v.x;
  position[1] = v.y;
  position[2] = v.z;

  rotation[0] = _LCR_racingSmoothRot(LCR_racing.carRotations[0].x,
    LCR_racing.carRotations[1].x);
  rotation[1] = _LCR_racingSmoothRot(LCR_racing.carRotations[0].y,
    LCR_racing.carRotations[1].y);
  rotation[2] = _LCR_racingSmoothRot(LCR_racing.carRotations[0].z,
    LCR_racing.carRotations[1].z);
#else
  position[0] = LCR_racing.carPositions[0].x;
  position[1] = LCR_racing.carPositions[0].y;
  position[2] = LCR_racing.carPositions[0].z;

  rotation[0] = LCR_racing.carRotations[0].x;
  rotation[1] = LCR_racing.carRotations[0].y;
  rotation[2] = LCR_racing.carRotations[0].z;
#endif
}

/**
  Gets the current block coordinates of the car, without interpolation etc.,
  intended for checking accurately whether CP has been taken etc.
*/
void LCR_racingGetCarBlockCoords(int coords[3])
{
  coords[0] = (LCR_racing.carPositions[0].x +
    (LCR_GAME_UNIT * LCR_MAP_SIZE_BLOCKS) / 2) / LCR_GAME_UNIT;

  coords[1] = (LCR_racing.carPositions[0].y +
    (LCR_GAME_UNIT * LCR_MAP_SIZE_BLOCKS) / 4) / (LCR_GAME_UNIT / 2);

  coords[2] = (LCR_racing.carPositions[0].z +
    (LCR_GAME_UNIT * LCR_MAP_SIZE_BLOCKS) / 2) / LCR_GAME_UNIT;
}

int LCR_racingCarWheelTouchesGround(int wheel)
{
  return ((LCR_racing.wheelCollisions & (LCR_racing.wheelCollisions >> 4))
    >> wheel) & 0x01;
}
      
LCR_GameUnit LCR_racingGetWheelRotation(void)
{
  return LCR_racing.wheelAngle;
}

LCR_GameUnit LCR_racingGetWheelSteer(void)
{
  return LCR_racing.wheelSteer;
}

TPE_Unit _LCR_applyMaterialFactor(TPE_Unit value, uint8_t mat)
{
  switch (mat)
  {
    case LCR_BLOCK_MATERIAL_GRASS:
      value *= LCR_CAR_GRASS_FACTOR;
      break;

    case LCR_BLOCK_MATERIAL_DIRT:
      value *= LCR_CAR_DIRT_FACTOR;
      break;

    case LCR_BLOCK_MATERIAL_ICE:
      value *= LCR_CAR_ICE_FACTOR;
      break;

    default: value *= 8; break;
  }

  return value / 8;
}

void _LCR_racingWheelAccelerate(unsigned int wheel, TPE_Vec3 dir,
  uint8_t material, uint8_t accelerator)
{
  TPE_Unit acc =
    _LCR_applyMaterialFactor(LCR_CAR_ACCELERATION,material);

  acc = acc / (1 + (LCR_racingGetCarSpeedUnsigned() / LCR_CAR_AIR_FRICTION));

  if (accelerator)
    acc *= 2; // TODO: constant?

  LCR_racing.carBody.joints[wheel].velocity[0] += (dir.x * acc) / TPE_F;
  LCR_racing.carBody.joints[wheel].velocity[1] += (dir.y * acc) / TPE_F;
  LCR_racing.carBody.joints[wheel].velocity[2] += (dir.z * acc) / TPE_F;
}

int _LCR_racingCarShapeOK(void)
{
  int r = 1;

  for (int i = 0; i < LCR_racing.carBody.jointCount; ++i)
    r &= TPE_connectionTension(TPE_dist(
        LCR_racing.carBody.joints[
          LCR_racing.carBody.connections[i].joint1].position,
        LCR_racing.carBody.joints[
          LCR_racing.carBody.connections[i].joint2].position),
      LCR_racing.carBody.connections[i].length) < TPE_F / 16; // TODO: const

  return r;
}

/**
  Updates the racing physics world, call every LCR_RACING_TICK_MS milliseconds.
  Returns a set of events (logically ORed) that occured during this step.
*/
uint32_t LCR_racingStep(unsigned int input)
{
  LCR_LOG2("racing step (start)");

  uint32_t result = 0;
  TPE_Vec3 carForw, carRight, carUp, carVel;
  uint8_t groundMat = LCR_BLOCK_MATERIAL_CONCRETE; // material under wheels
  uint8_t onAccel = 0; // standing on accelerator?
  int groundBlockIndex = -1;

  if (LCR_racing.playingReplay)
  {
    if (LCR_racing.tick == 0)
      LCR_replayInitPlaying();

    input = LCR_replayGetNextInput();

  }
  else
  {
    if (LCR_racing.tick == 0)
      LCR_replayInitRecording();

    LCR_replayRecordEvent(LCR_racing.tick,input);
  }

  carForw = TPE_vec3Normalized(TPE_vec3Plus(
    TPE_vec3Minus(LCR_racing.carBody.joints[0].position,
                  LCR_racing.carBody.joints[2].position),
    TPE_vec3Minus(LCR_racing.carBody.joints[1].position,
                  LCR_racing.carBody.joints[3].position)));

  carRight = TPE_vec3Normalized(TPE_vec3Plus(
    TPE_vec3Minus(LCR_racing.carBody.joints[0].position,
                  LCR_racing.carBody.joints[1].position),
    TPE_vec3Minus(LCR_racing.carBody.joints[2].position,
                  LCR_racing.carBody.joints[3].position)));

  carUp = TPE_vec3Cross(carForw,carRight);

  carVel = TPE_vec3(
    LCR_racing.carBody.joints[4].velocity[0],
    LCR_racing.carBody.joints[4].velocity[1],
    LCR_racing.carBody.joints[4].velocity[2]);

  if ((LCR_racing.wheelCollisions & 0x0f) != 0x0f) // EXPERIMENTAL: don't apply gravity with all wheels on ground
    TPE_bodyApplyGravity(&(LCR_racing.carBody),LCR_GRAVITY);

  if (LCR_racing.wheelCollisions) // at least one wheel on ground?
  {
    TPE_Unit upDot = TPE_vec3Dot(carUp,TPE_vec3(0,TPE_F,0));

    if (upDot > TPE_F / 8 || upDot < -1 * TPE_F / 8) // TODO: consts
    {
      uint8_t
        gx = (LCR_racing.carPositions[0].x + (LCR_MAP_SIZE_BLOCKS / 2) *
          LCR_GAME_UNIT) / LCR_GAME_UNIT,
        gy = (LCR_racing.carPositions[0].y + (LCR_MAP_SIZE_BLOCKS / 2) *
          (LCR_GAME_UNIT / 2)) / (LCR_GAME_UNIT / 2),
        gz = (LCR_racing.carPositions[0].z + (LCR_MAP_SIZE_BLOCKS / 2) *
          LCR_GAME_UNIT) / LCR_GAME_UNIT;

      TPE_Unit yMod = (LCR_racing.carPositions[0].y + LCR_MAP_SIZE_BLOCKS *
        LCR_GAME_UNIT / 2) % (LCR_GAME_UNIT / 2);

      if (upDot > 0 && yMod < LCR_GAME_UNIT / 2) // TODO: const
        groundBlockIndex = LCR_mapGetBlockAt(gx,gy - 1,gz);
      else if (upDot < 0 && yMod > LCR_GAME_UNIT / 2)
        groundBlockIndex = LCR_mapGetBlockAt(gx,gy + 1,gz);

      if (groundBlockIndex == -1)
        groundBlockIndex = LCR_mapGetBlockAt(gx,gy,gz);
    }

    if (groundBlockIndex != -1)
    {
      uint8_t b = LCR_currentMap.blocks[groundBlockIndex * LCR_BLOCK_SIZE];

      onAccel = LCR_mapBlockIsAccelerator(b);

      if (LCR_mapBlockIsFan(b))
        LCR_racing.fanForce = LCR_GRAVITY * LCR_FAN_FORCE;

      groundMat = LCR_mapBlockGetMaterial(
        LCR_currentMap.blocks + groundBlockIndex * LCR_BLOCK_SIZE);
    }
  }

  LCR_racing.carBody.friction =
    _LCR_applyMaterialFactor(LCR_CAR_FORWARD_FRICTION,groundMat);

  if (onAccel)
    input |= LCR_RACING_INPUT_FORW; // accelerator enforces this

  if(!(input & (LCR_RACING_INPUT_FORW | LCR_RACING_INPUT_BACK)))
    LCR_racing.carBody.friction *= LCR_CAR_STAND_FRICTION_MULTIPLIER;
  else if (
  ((input & LCR_RACING_INPUT_FORW) && (LCR_racing.carSpeeds[0] < 0)) ||
  ((input & LCR_RACING_INPUT_BACK) && (LCR_racing.carSpeeds[0] > 0)))
    LCR_racing.carBody.friction *= 2 * LCR_CAR_STAND_FRICTION_MULTIPLIER;

  if (input)
  {
    unsigned char steering = 0;
  
    if ((input & LCR_RACING_INPUT_BACK) &&
      (LCR_racing.wheelCollisions & 0x0f) == 0x00) // in air?
    {
      LCR_LOG2("air brake");

      for (int i = 0; i < LCR_CAR_JOINTS - 1; ++i)
        for (int j = 0; j < 3; ++j)
          LCR_racing.carBody.joints[i].velocity[j] =  
            LCR_racing.carBody.joints[LCR_CAR_JOINTS - 1].velocity[j]; 
    }

    // TODO: magic constants

    if (input & (LCR_RACING_INPUT_FORW | LCR_RACING_INPUT_BACK))
    {
      LCR_GameUnit rotateBy =
        (LCR_racing.wheelCollisions & 0x0f) ? // on ground slow down wheel rot.
        (LCR_racingGetCarSpeedUnsigned() / 8) : LCR_GAME_UNIT / 32;

      if (!(input & LCR_RACING_INPUT_BACK))
        rotateBy *= -1;

      LCR_racing.wheelAngle =
        (LCR_racing.wheelAngle + rotateBy) % LCR_GAME_UNIT;

      if (LCR_racing.wheelAngle < 0)
        LCR_racing.wheelAngle += LCR_GAME_UNIT;
    }

    if (input & LCR_RACING_INPUT_RIGHT)
    {
      steering = 2;
      
      LCR_racing.wheelSteer = TPE_min(
        LCR_racing.wheelSteer + LCR_CAR_STEER_SPEED,
        LCR_CAR_STEER_MAX);
    }
    else if (input & LCR_RACING_INPUT_LEFT)
    {
      steering = 1;

      LCR_racing.wheelSteer = TPE_max(
        LCR_racing.wheelSteer - LCR_CAR_STEER_SPEED,
        -1 * LCR_CAR_STEER_MAX);
    }

    if ((LCR_racing.wheelCollisions & 0x0c)) // back wheel on ground?
    {
      if (input & LCR_RACING_INPUT_FORW)
      {
        _LCR_racingWheelAccelerate(0,carForw,groundMat,onAccel);
        _LCR_racingWheelAccelerate(1,carForw,groundMat,onAccel);
      }
      else if (input & LCR_RACING_INPUT_BACK)
      {
        _LCR_racingWheelAccelerate(0,TPE_vec3TimesPlain(carForw,-1),groundMat,onAccel);
        _LCR_racingWheelAccelerate(1,TPE_vec3TimesPlain(carForw,-1),groundMat,onAccel);
      }
    }

    TPE_Unit driftFriction = 0; // average wheel friction (absolute value)

    for (int i = 0; i < 4; ++i)
      if (LCR_racing.wheelCollisions & (0x01 << i)) // wheel on ground?
      {
        TPE_Vec3 jv = TPE_vec3( // joint velocity 
          LCR_racing.carBody.joints[i].velocity[0],   
          LCR_racing.carBody.joints[i].velocity[1],   
          LCR_racing.carBody.joints[i].velocity[2]);   

        TPE_Vec3 ja = carRight; // wheel axis of rotation

        if (i >= 2 && steering)
        {
          // for front wheels with turning we tilt the wheel axis 45 degrees

          TPE_Unit steer =
            (LCR_racing.wheelSteer * TPE_F) / LCR_GAME_UNIT;
 
          ja = TPE_vec3Normalized(
            TPE_vec3Plus(TPE_vec3Times(carForw,steer),carRight));
        }

        /* friction is in the direction if the axis and its magnitude is
          determined by the dot product (angle) of the axis and velocity */

        TPE_Vec3 fric = TPE_vec3Times(ja,(TPE_vec3Dot(ja,jv) *
          _LCR_applyMaterialFactor(
            LCR_racing.carDrifting ?
              (LCR_CAR_STEER_FRICTION * LCR_CAR_DRIFT_FACTOR) / 8 :
              LCR_CAR_STEER_FRICTION,groundMat)) / TPE_F);

        driftFriction += TPE_vec3Len(fric);

        jv = TPE_vec3Minus(jv,fric); // subtract the friction

        LCR_racing.carBody.joints[i].velocity[0] = jv.x;
        LCR_racing.carBody.joints[i].velocity[1] = jv.y;
        LCR_racing.carBody.joints[i].velocity[2] = jv.z;
      }

      driftFriction /= 4;

      if ((!LCR_racing.carDrifting) &&
        driftFriction > LCR_CAR_DRIFT_THRESHOLD_1)
      {
        LCR_LOG1("drift start");
        LCR_racing.carDrifting = 1;
      }
      else if (LCR_racing.carDrifting &&
        driftFriction < LCR_CAR_DRIFT_THRESHOLD_0)
      {
        LCR_LOG1("drift end");
        LCR_racing.carDrifting = 0;
      }

      /* The following fixes "sticking to a wall" issue by adding a small spin
        to the car when trying to steer at very small velocity. */
      if (steering &&
        (input & (LCR_RACING_INPUT_FORW | LCR_RACING_INPUT_BACK)) &&
        (LCR_racing.wheelCollisions & 0x0f) &&
        LCR_racingGetCarSpeedUnsigned() < (LCR_GAME_UNIT / 25))
      {
        LCR_LOG2("spinning car a bit");

        TPE_bodySpin(&LCR_racing.carBody,TPE_vec3(0,
          steering == ((input & LCR_RACING_INPUT_LEFT) != 0) ?
          TPE_F / 32 : -1 * TPE_F / 32,0));
      }
  }

  if ((!(input & LCR_RACING_INPUT_LEFT)) &&
    (!(input & LCR_RACING_INPUT_RIGHT)))
    LCR_racing.wheelSteer /= 2;

  LCR_racing.wheelCollisions <<= 4;

  if (LCR_racing.fanForce)
  {
    TPE_bodyAccelerate(&(LCR_racing.carBody),TPE_vec3(0,LCR_racing.fanForce,0));

    LCR_racing.fanForce -= LCR_GRAVITY / LCR_FAN_FORCE_DECREASE;

    if (LCR_racing.fanForce < 0)
      LCR_racing.fanForce = 0;
  }

  LCR_LOG2("stepping physics (start)");
  TPE_worldStep(&(LCR_racing.physicsWorld));
  LCR_LOG2("stepping physics (end)");

  int speedDiff = 
    TPE_abs(LCR_racing.carSpeeds[0]) - 
    TPE_abs(LCR_racing.carSpeeds[1]);

  LCR_racing.carSpeeds[1] = LCR_racing.carSpeeds[0];

  LCR_racing.carSpeeds[0] = (TPE_vec3Len(carVel) * LCR_GAME_UNIT)
    / LCR_PHYSICS_UNIT;

  if (TPE_vec3Dot(carVel,carForw) < 0)
    LCR_racing.carSpeeds[0] *= -1;

  if (speedDiff < -1 * LCR_CAR_CRASH_SPEED_DIFF &&
    TPE_abs(LCR_racing.carSpeeds[0]) <= LCR_CAR_CRASH_SPEED_THRESHOLD)
    result |= (speedDiff < -2 * LCR_CAR_CRASH_SPEED_DIFF) ?
      LCR_RACING_EVENT_CRASH_BIG : LCR_RACING_EVENT_CRASH_SMALL;

  _LCR_racingUpdateCarPosRot();

  TPE_Unit angle = TPE_vec3Dot(carUp,TPE_vec3Normalized(TPE_vec3Minus(
    LCR_racing.carBody.joints[4].position,
    LCR_racing.carBody.joints[0].position)));

  if (angle < TPE_F / 64) // TODO: magic constant
  {
    LCR_LOG2("roof squeezed, applying anti force")

    TPE_Vec3 tmpVec = TPE_vec3Times(carUp,LCR_PHYSICS_UNIT / 16); // TODO: 16 magic con.

    angle = TPE_F - 4 * angle; // 4 comes from above TPE_F / 4

    tmpVec = TPE_vec3Times(tmpVec,angle);

    if (angle <= 0)  
    {
      LCR_LOG1("roof flipped over, fixing")
      LCR_racing.carBody.joints[4].position = _LCR_racingGetWheelCenterPoint();
      angle = 0;
    }

    // accelerate roof and wheels away from each other
    for (int i = 0; i < LCR_CAR_JOINTS; ++i)
    {
      LCR_racing.carBody.joints[i].velocity[0] += (i == 4 ? 1 : -1) * tmpVec.x;
      LCR_racing.carBody.joints[i].velocity[1] += (i == 4 ? 1 : -1) * tmpVec.y;
      LCR_racing.carBody.joints[i].velocity[2] += (i == 4 ? 1 : -1) * tmpVec.z;
    }
  }

  // now make a special test for a "pinch" front collision

  TPE_Vec3 frontWheelMiddle = TPE_vec3Plus(
    LCR_racing.carBody.joints[0].position,
    LCR_racing.carBody.joints[1].position);

  frontWheelMiddle.x /= 2;
  frontWheelMiddle.y /= 2;
  frontWheelMiddle.z /= 2;

  frontWheelMiddle = TPE_vec3Minus(frontWheelMiddle,
    _LCR_racingEnvironmentFunction(frontWheelMiddle,LCR_PHYSICS_UNIT / 4));

  uint8_t frontCollision = 
    carForw.y < TPE_FRACTIONS_PER_UNIT / 4 && // only when car is horizontal
    carForw.y > -1 * TPE_FRACTIONS_PER_UNIT / 4 &&
    frontWheelMiddle.x == 0 && frontWheelMiddle.y == 0 &&
    frontWheelMiddle.z == 0;

  if (frontCollision)
  {
    // stop the car immediately
    LCR_LOG1("car front pierced");

    LCR_racing.carNotOKCount +=
      (LCR_racing.carNotOKCount < 20 ? 15 : 0); // TODO: consts
  }

  if ((LCR_racing.carBody.flags & TPE_BODY_FLAG_UNRESOLVED) || frontCollision ||
    !_LCR_racingCarShapeOK())
  {
    // car not OK

    if (LCR_racing.carNotOKCount > 10) // TODO: constant
    {
      LCR_LOG1("car not OK (short), fixing");

      for (int i = 0; i < LCR_CAR_JOINTS; ++i)
      {
        if (LCR_racing.carNotOKCount < 20) // TODO: const
        {
          // for a while try to smoothly iterate towards previous OK position 
          LCR_racing.carBody.joints[i].position = 
            TPE_vec3Plus(LCR_racing.carBody.joints[i].position,
              LCR_racing.carOKPositions[i]);

          LCR_racing.carBody.joints[i].position.x /= 2;
          LCR_racing.carBody.joints[i].position.y /= 2;
          LCR_racing.carBody.joints[i].position.z /= 2;

          for (int j = 0; j < 3; ++j) // lower speed a bit
            LCR_racing.carBody.joints[i].velocity[j] = 
              (7 * ((int) LCR_racing.carBody.joints[i].velocity[j])) / 8;
        }
        else // hard set the pos (iteration may be infinite due to sim.)
        {
          LCR_LOG1("car not OK (long), teleporting");
          LCR_racing.carBody.joints[i].position = LCR_racing.carOKPositions[i];
        }
      }
    }

    LCR_racing.carNotOKCount += LCR_racing.carNotOKCount < 255 ? 1 : 0;
  }
  else
  {
    // car OK
    LCR_racing.carNotOKCount = 0;

    for (int i = 0; i < LCR_CAR_JOINTS; ++i)
      LCR_racing.carOKPositions[i] = LCR_racing.carBody.joints[i].position;
  }

  int carBlock[3];

  LCR_racingGetCarBlockCoords(carBlock);

  carBlock[0] = LCR_mapGetBlockAt(carBlock[0],carBlock[1],carBlock[2]);

  if (carBlock[0] >= 0)
  {
    if (LCR_currentMap.blocks[carBlock[0] * LCR_BLOCK_SIZE] ==
      LCR_BLOCK_CHECKPOINT_0)
    {
      LCR_currentMap.blocks[carBlock[0] * LCR_BLOCK_SIZE] =
        LCR_BLOCK_CHECKPOINT_1;

      result |= LCR_RACING_EVENT_CP_TAKEN;
    }
    else if (LCR_currentMap.blocks[carBlock[0] * LCR_BLOCK_SIZE] ==
      LCR_BLOCK_FINISH)
    {
      int valid = 1;

      for (int i = 0; i < LCR_currentMap.blockCount; ++i)
        if (LCR_currentMap.blocks[i * LCR_BLOCK_SIZE] == LCR_BLOCK_CHECKPOINT_0)
        {
          valid = 0;
          break;
        }

      if (valid)
      {
        result |= LCR_RACING_EVENT_FINISHED;

        if (!LCR_racing.playingReplay)
          LCR_replayRecordEvent(LCR_racing.tick,LCR_REPLAY_EVENT_END);
      }
    }
  }

  LCR_racing.tick += LCR_racing.tick < 0xffffffff; // disallow overflow

  LCR_LOG2("racing step (end)");

  return result;
}

/**
  Draws a simple 3D debug overlap of the physics world.
*/
void LCR_physicsDebugDraw(LCR_GameUnit camPos[3], LCR_GameUnit camRot[2],
  LCR_GameUnit camFov,
  void (*drawPixel)(uint16_t, uint16_t, uint8_t))
{
#if LCR_SETTING_DEBUG_PHYSICS_DRAW
  LCR_LOG2("drawing physics debug");

  TPE_Vec3 cPos, cRot, cView;

  cPos.x = (camPos[0] * LCR_PHYSICS_UNIT) / LCR_GAME_UNIT;
  cPos.y = (camPos[1] * LCR_PHYSICS_UNIT) / LCR_GAME_UNIT;
  cPos.z = (camPos[2] * LCR_PHYSICS_UNIT) / LCR_GAME_UNIT;

  cRot.x = (camRot[0] * TPE_F) / LCR_GAME_UNIT;
  cRot.y = (camRot[1] * TPE_F) / LCR_GAME_UNIT;
  cRot.z = 0; 

  cView.x = LCR_EFFECTIVE_RESOLUTION_X;
  cView.y = LCR_EFFECTIVE_RESOLUTION_Y;
  cView.z = (camFov * TPE_F) / LCR_GAME_UNIT;

  TPE_worldDebugDraw(&(LCR_racing.physicsWorld),drawPixel,
    cPos,cRot,cView,16,LCR_PHYSICS_UNIT / 4,LCR_racing.tick * 4);

/*
  TPE_worldDebugDraw(&(LCR_racing.physicsWorld),_LCR_drawPhysicsDebugPixel,
    cPos,cRot,cView,16,LCR_PHYSICS_UNIT / 4,LCR_racing.tick * 4);
*/
#endif
}

#endif // guard