btHfFluid.cpp

Go to the documentation of this file.

/*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.com

 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
 Permission is granted to anyone to use this software for any purpose,
 including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:

 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
 3. This notice may not be removed or altered from any source distribution.

 Experimental Buoyancy fluid demo written by John McCutchan
 */

#include <stdio.h>
#include <iostream>
#include <uwsim/BulletHfFluid/btHfFluid.h>
#include <uwsim/BulletHfFluid/btHfFluidCollisionShape.h>
#include <uwsim/BulletHfFluid/btHfFluidBuoyantConvexShape.h>
#include <BulletCollision/NarrowPhaseCollision/btPersistentManifold.h>
#include <BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h>
#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
#include <BulletCollision/CollisionDispatch/btManifoldResult.h>
#include <BulletCollision/CollisionShapes/btTriangleShape.h>
#include <BulletDynamics/Dynamics/btRigidBody.h>
//#include "../../OpenGL/GLDebugDrawer.h"

btHfFluid::btHfFluid(btScalar gridCellWidth, int numNodesWidth, int numNodesLength)
{
  m_eta = NULL;
  m_temp = NULL;
  m_ground = NULL;
  m_height[0] = NULL;
  m_height[1] = NULL;
  m_u[0] = NULL;
  m_u[1] = NULL;
  m_v[0] = NULL;
  m_v[1] = NULL;
  m_r[0] = NULL;
  m_r[1] = NULL;
  m_flags = NULL;
  m_fillRatio = NULL;
  m_internalType = CO_HF_FLUID;
  m_heightIndex = 0;
  m_velocityIndex = 0;
  m_rIndex = 0;
  setGridDimensions(gridCellWidth, numNodesWidth, numNodesLength);

  btScalar maxHeight = 20.0;
  m_aabbMin = btVector3(0.0, 0.0, -10.0);
  m_aabbMax = btVector3(m_gridWidth, m_gridLength, maxHeight);

  setCollisionShape(new btHfFluidCollisionShape(this));

  m_globalVelocityU = btScalar(0.0f);
  m_globalVelocityV = btScalar(0.0f);
  m_gravity = btScalar(-1.0f);

  m_volumeDisplacementScale = btScalar(0.5f);
  m_horizontalVelocityScale = btScalar(0.5f);

  m_epsEta = btScalar(0.01f);
  m_epsHeight = btScalar(0.001f);
}

btHfFluid::btHfFluid(btScalar gridCellWidth, int minX, int maxX, int minY, int maxY, int depth, int height)
{
  m_eta = NULL;
  m_temp = NULL;
  m_ground = NULL;
  m_height[0] = NULL;
  m_height[1] = NULL;
  m_u[0] = NULL;
  m_u[1] = NULL;
  m_v[0] = NULL;
  m_v[1] = NULL;
  m_r[0] = NULL;
  m_r[1] = NULL;
  m_flags = NULL;
  m_fillRatio = NULL;
  m_internalType = CO_HF_FLUID;
  m_heightIndex = 0;
  m_velocityIndex = 0;
  m_rIndex = 0;

  setGridDimensions(gridCellWidth, ceil(maxX - minX) / gridCellWidth, ceil(maxY - minY) / gridCellWidth);

  m_aabbMin = btVector3(minX, minY, depth);
  m_aabbMax = btVector3(maxX, maxY, height);

  setCollisionShape(new btHfFluidCollisionShape(this));

  m_globalVelocityU = btScalar(0.0f);
  m_globalVelocityV = btScalar(0.0f);
  m_gravity = btScalar(-1.0f);

  m_volumeDisplacementScale = btScalar(0.5f);
  m_horizontalVelocityScale = btScalar(0.5f);

  m_epsEta = btScalar(0.01f);
  m_epsHeight = btScalar(0.001f);
}

btHfFluid::~btHfFluid()
{
  btCollisionShape* collisionShape = getCollisionShape();
  delete collisionShape;
  btAlignedFree (m_temp);
  btAlignedFree (m_height[0]);
  btAlignedFree(m_height[1]);
  btAlignedFree (m_ground);
  btAlignedFree (m_eta);
  btAlignedFree (m_u[0]);
  btAlignedFree(m_u[1]);
  btAlignedFree (m_v[0]);
  btAlignedFree(m_v[1]);
  btAlignedFree (m_flags);
  btAlignedFree (m_fillRatio);
}

void btHfFluid::predictMotion(btScalar dt)
{
  transferDisplaced(dt);

  advectEta(dt);
  advectVelocityU(dt);
  advectVelocityV(dt);
  updateHeight(dt);
  computeFlagsAndFillRatio();
  updateVelocity(dt);
  setReflectBoundaryLeft();
  setReflectBoundaryRight();
  setReflectBoundaryTop();
  setReflectBoundaryBottom();
  debugTests();
  //m_heightIndex = (m_heightIndex + 1) % 2;
  //m_velocityIndex = (m_velocityIndex + 1) % 2;
}

void btHfFluid::prep()
{
  for (int i = 0; i < m_numNodesLength * m_numNodesWidth; i++)
  {
    m_height[0][i] = m_eta[i] + m_ground[i];
    m_height[1][i] = m_eta[i] + m_ground[i];
  }
  computeFlagsAndFillRatio();
}

btScalar btHfFluid::widthPos(int i) const
{
  return m_gridCellWidth * i + m_aabbMin.getX();
}

btScalar btHfFluid::lengthPos(int j) const
{
  return m_gridCellWidth * j + m_aabbMin.getX();
}

int btHfFluid::arrayIndex(int i, int j) const
{
  btAssert(i >= 0);
  btAssert(i < m_numNodesWidth);
  btAssert(j >= 0);
  btAssert(j < m_numNodesLength);
  int index = i + (j * m_numNodesWidth);
  return index;
}

int btHfFluid::arrayIndex(btScalar i, btScalar j) const
{
  int ii = (int)i; // truncate / floor
  int ij = (int)j; // truncate / floor

  return arrayIndex(ii, ij);
}

const btScalar* btHfFluid::getHeightArray() const
{
  return m_height[m_heightIndex];
}

const btScalar* btHfFluid::getGroundArray() const
{
  return m_ground;
}
const btScalar* btHfFluid::getEtaArray() const
{
  return m_eta;
}
const btScalar* btHfFluid::getVelocityUArray() const
{
  return m_u[m_velocityIndex];
}
const btScalar* btHfFluid::getVelocityVArray() const
{
  return m_v[m_velocityIndex];
}

const bool* btHfFluid::getFlagsArray() const
{
  return m_flags;
}

btScalar* btHfFluid::getHeightArray()
{
  return m_height[m_heightIndex];
}
btScalar* btHfFluid::getGroundArray()
{
  return m_ground;
}
btScalar* btHfFluid::getEtaArray()
{
  return m_eta;
}
btScalar* btHfFluid::getVelocityUArray()
{
  return m_u[m_velocityIndex];
}
btScalar* btHfFluid::getVelocityVArray()
{
  return m_v[m_velocityIndex];
}

bool* btHfFluid::getFlagsArray()
{
  return m_flags;
}

void btHfFluid::setFluidHeight(int x, int y, btScalar height)
{
  int index = arrayIndex(x, y);
  setFluidHeight(index, height);
}

void btHfFluid::setFluidHeight(int index, btScalar height)
{
  m_eta[index] = height - m_ground[index];
  m_height[m_heightIndex][index] = m_ground[index] + m_eta[index];
  m_flags[index] = true;
}

void btHfFluid::addFluidHeight(int x, int y, btScalar height)
{
  int index = arrayIndex(x, y);
  m_eta[index] += height;
  m_height[m_heightIndex][index] = m_ground[index] + m_eta[index];
  m_flags[index] = true;
}

void btHfFluid::getAabbForColumn(int i, int j, btVector3& aabbMin, btVector3& aabbMax)
{
  btVector3 com = getWorldTransform().getOrigin();
  int sw = arrayIndex(i, j);
  int se = arrayIndex(i + 1, j);
  int nw = arrayIndex(i, j + 1);
  int ne = arrayIndex(i + 1, j + 1);

  btScalar h = m_height[m_heightIndex][sw];
  btScalar g = m_ground[sw];

  aabbMin = btVector3(widthPos(i), lengthPos(j), g);
  aabbMax = btVector3(widthPos(i + 1), lengthPos(j + 1), h);

  aabbMin += com;
  aabbMax += com;
}

void btHfFluid::foreachGroundTriangle(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax)
{
  btVector3 verts[3];

  btScalar minX, minZ, maxX, maxZ;
  int startNodeX, startNodeZ, endNodeX, endNodeZ;

  minX = aabbMin.getX();
  minZ = aabbMin.getY();
  maxX = aabbMax.getX();
  maxZ = aabbMax.getY();

  startNodeX = (int)((minX - m_aabbMin.getX()) * m_gridCellWidthInv);
  startNodeZ = (int)((minZ - m_aabbMin.getY()) * m_gridCellWidthInv);

  endNodeX = (int)((maxX - m_aabbMin.getX()) * m_gridCellWidthInv);
  endNodeZ = (int)((maxZ - m_aabbMin.getY()) * m_gridCellWidthInv);

  endNodeX++;
  endNodeZ++;

  startNodeX = btMax(1, startNodeX);
  startNodeZ = btMax(1, startNodeZ);
  endNodeX = btMin(m_numNodesWidth - 1, endNodeX);
  endNodeZ = btMin(m_numNodesLength - 1, endNodeZ);

#ifdef __BRUTE__
  for (int i = 1; i < m_numNodesWidth-1; i++)
  {
    for (int j = 1; j < m_numNodesLength-1; j++)
    {
      // triangle 1
      verts[0] = btVector3(widthPos(i), m_ground[arrayIndex(i,j)], lengthPos(j));
      verts[1] = btVector3(widthPos(i), m_ground[arrayIndex(i,j+1)], lengthPos(j+1));
      verts[2] = btVector3(widthPos(i+1), m_ground[arrayIndex(i+1,j)], lengthPos(j));
      callback->processTriangle(verts,i,j);
      // triangle 2
      verts[0] = btVector3(widthPos(i+1), m_ground[arrayIndex(i+1,j)], lengthPos(j));
      verts[1] = btVector3(widthPos(i), m_ground[arrayIndex(i,j+1)], lengthPos(j+1));
      verts[2] = btVector3(widthPos(i+1), m_ground[arrayIndex(i+1,j+1)], lengthPos(j+1));
      callback->processTriangle(verts,i,j);
    }
  }
#else

  for (int i = startNodeX; i < endNodeX; i++)
  {
    for (int j = startNodeZ; j < endNodeZ; j++)
    {
      // triangle 1
      verts[0] = btVector3(widthPos(i), m_ground[arrayIndex(i, j)], lengthPos(j));
      verts[1] = btVector3(widthPos(i), m_ground[arrayIndex(i, j + 1)], lengthPos(j + 1));
      verts[2] = btVector3(widthPos(i + 1), m_ground[arrayIndex(i + 1, j)], lengthPos(j));
      callback->processTriangle(verts, i, j);
      // triangle 2
      verts[0] = btVector3(widthPos(i + 1), m_ground[arrayIndex(i + 1, j)], lengthPos(j));
      verts[1] = btVector3(widthPos(i), m_ground[arrayIndex(i, j + 1)], lengthPos(j + 1));
      verts[2] = btVector3(widthPos(i + 1), m_ground[arrayIndex(i + 1, j + 1)], lengthPos(j + 1));
      callback->processTriangle(verts, i, j);
    }
  }
#endif
}

void btHfFluid::foreachFluidColumn(btHfFluidColumnCallback* callback, const btVector3& aabbMin,
                                   const btVector3& aabbMax)
{
  btScalar minX, minZ, maxX, maxZ;
  int startNodeX, startNodeZ, endNodeX, endNodeZ;

  minX = aabbMin.getX();
  minZ = aabbMin.getY();
  maxX = aabbMax.getX();
  maxZ = aabbMax.getY();

  startNodeX = (int)((minX - m_aabbMin.getX()) * m_gridCellWidthInv);
  startNodeZ = (int)((minZ - m_aabbMin.getY()) * m_gridCellWidthInv);

  endNodeX = (int)((maxX - m_aabbMin.getX()) * m_gridCellWidthInv);
  endNodeZ = (int)((maxZ - m_aabbMin.getY()) * m_gridCellWidthInv);

  endNodeX++;
  endNodeZ++;

  startNodeX = btMax(1, startNodeX);
  startNodeZ = btMax(1, startNodeZ);
  endNodeX = btMin(m_numNodesWidth - 2, endNodeX);
  endNodeZ = btMin(m_numNodesLength - 2, endNodeZ);

  bool r;
  for (int i = startNodeX; i < endNodeX; i++)
  {
    for (int j = startNodeZ; j < endNodeZ; j++)
    {
      if (m_flags[arrayIndex(i, j)] == false)
        continue;

      r = callback->processColumn(this, i, j);
      if (!r)
        return;
    }
  }
}

void btHfFluid::foreachSurfaceTriangle(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax)
{
  btVector3 verts[3];

  btScalar minX, minZ, maxX, maxZ;
  int startNodeX, startNodeZ, endNodeX, endNodeZ;

  minX = aabbMin.getX();
  minZ = aabbMin.getY();
  maxX = aabbMax.getX();
  maxZ = aabbMax.getY();

  startNodeX = (int)((minX - m_aabbMin.getX()) * m_gridCellWidthInv);
  startNodeZ = (int)((minZ - m_aabbMin.getY()) * m_gridCellWidthInv);

  endNodeX = (int)((maxX - m_aabbMin.getX()) * m_gridCellWidthInv);
  endNodeZ = (int)((maxZ - m_aabbMin.getY()) * m_gridCellWidthInv);

  endNodeX++;
  endNodeZ++;

  startNodeX = btMax(1, startNodeX);
  startNodeZ = btMax(1, startNodeZ);
  endNodeX = m_numNodesWidth - 1;
  endNodeZ = m_numNodesLength - 1;

  for (int i = startNodeX; i < endNodeX; i++)
  {
    for (int j = startNodeZ; j < endNodeZ; j++)
    {
      if (!m_flags[arrayIndex(i, j)])
        continue;
      // triangle 1
      verts[0] = btVector3(widthPos(i), m_height[m_heightIndex][arrayIndex(i, j)], lengthPos(j));
      verts[1] = btVector3(widthPos(i), m_height[m_heightIndex][arrayIndex(i, j + 1)], lengthPos(j + 1));
      verts[2] = btVector3(widthPos(i + 1), m_height[m_heightIndex][arrayIndex(i + 1, j)], lengthPos(j));
      callback->processTriangle(verts, i, j);
      // triangle 2
      verts[0] = btVector3(widthPos(i + 1), m_height[m_heightIndex][arrayIndex(i + 1, j)], lengthPos(j));
      verts[1] = btVector3(widthPos(i), m_height[m_heightIndex][arrayIndex(i, j + 1)], lengthPos(j + 1));
      verts[2] = btVector3(widthPos(i + 1), m_height[m_heightIndex][arrayIndex(i + 1, j + 1)], lengthPos(j + 1));
      callback->processTriangle(verts, i, j);
    }
  }
}

void btHfFluid::setGridDimensions(btScalar gridCellWidth, int numNodesWidth, int numNodesLength)
{
  m_gridWidth = gridCellWidth * numNodesWidth;
  m_gridLength = gridCellWidth * numNodesLength;
  m_gridCellWidth = gridCellWidth;
  m_numNodesWidth = numNodesWidth;
  m_numNodesLength = numNodesLength;
  m_gridCellWidthInv = btScalar(1.0) / gridCellWidth;

  allocateArrays();
}

btScalar btHfFluid::bilinearInterpolate(const btScalar* array, btScalar iPos, btScalar jPos)
{
  int i = (int)iPos;
  int j = (int)jPos;

  btScalar iParam1 = iPos - btScalar(i);
  btScalar iParam0 = btScalar(1.0) - iParam1;

  btScalar jParam1 = jPos - btScalar(j);
  btScalar jParam0 = btScalar(1.0) - jParam1;

  btScalar SW = array[arrayIndex(i, j)];
  btScalar SE = array[arrayIndex(i + 1, j)];
  btScalar NW = array[arrayIndex(i, j + 1)];
  btScalar NE = array[arrayIndex(i + 1, j + 1)];

  btScalar a = jParam0 * SW + jParam1 * NW;
  btScalar b = jParam0 * SE + jParam1 * NE;
  return iParam0 * a + iParam1 * b;
}

btScalar btHfFluid::advect(const btScalar* array, btScalar i, btScalar j, btScalar di, btScalar dj, btScalar dt)
{
  // trace particle backwards in time
  btScalar srcI = i - di * dt * m_gridCellWidthInv;
  btScalar srcJ = j - dj * dt * m_gridCellWidthInv;

  // srcI and srcJ are indices into the array,
  // we need to clamp them to within the domain
  srcI = btMax(srcI, btScalar(0.0));
  srcI = btMin(srcI, btScalar(m_numNodesWidth - 1));
  srcJ = btMax(srcJ, btScalar(0.0));
  srcJ = btMin(srcJ, btScalar(m_numNodesLength - 1));

  return bilinearInterpolate(array, srcI, srcJ);
}

void btHfFluid::advectEta(btScalar dt)
{
  for (int i = 1; i < m_numNodesWidth - 1; i++)
  {
    for (int j = 1; j < m_numNodesLength - 1; j++)
    {
      int index = arrayIndex(i, j);

      if (!m_flags[index])
        continue;

      btScalar u = m_globalVelocityU;
      btScalar v = m_globalVelocityV;

      u += (m_u[m_velocityIndex][index] + m_u[m_velocityIndex][index + 1]) * btScalar(0.5);
      v += (m_v[m_velocityIndex][index] + m_v[m_velocityIndex][index + m_numNodesWidth]) * btScalar(0.5);

      m_temp[index] = advect(m_eta, btScalar(i), btScalar(j), u, v, dt);
    }
  }
  for (int i = 1; i < m_numNodesWidth - 1; i++)
  {
    for (int j = 1; j < m_numNodesLength - 1; j++)
    {
      int index = arrayIndex(i, j);
      m_eta[index] = m_temp[index];
    }
  }
}

void btHfFluid::updateHeight(btScalar dt)
{
  for (int j = 1; j < m_numNodesLength - 1; j++)
  {
    for (int i = 1; i < m_numNodesWidth - 1; i++)
    {
      int index = arrayIndex(i, j);
      if (!m_flags[index])
      {
        m_height[m_heightIndex][index] = m_ground[index] + m_eta[index];
        continue;
      }
      btScalar deta = -btScalar(0.5f) * m_eta[index] * dt * m_gridCellWidthInv
          * ((m_u[m_velocityIndex][index + 1] - m_u[m_velocityIndex][index])
              + (m_v[m_velocityIndex][index + m_numNodesWidth] - m_v[m_velocityIndex][index]));
      m_eta[index] += deta;
      m_height[m_heightIndex][index] = m_ground[index] + btMax(m_eta[index], btScalar(0.0f));
    }
  }
}

void btHfFluid::advectVelocityU(btScalar dt)
{
  for (int i = 1; i < m_numNodesWidth - 1; i++)
  {
    for (int j = 1; j < m_numNodesLength - 1; j++)
    {
      int index = arrayIndex(i, j);
      if (!m_flags[index])
      {
        continue;
      }
      btScalar u = m_globalVelocityU;
      btScalar v = m_globalVelocityV;

      u += m_u[m_velocityIndex][index];
      v += (m_v[m_velocityIndex][index] + m_v[m_velocityIndex][index + 1]
          + m_v[m_velocityIndex][index + m_numNodesWidth] + m_v[m_velocityIndex][index + m_numNodesWidth + 1])
          * btScalar(0.25);

      m_temp[index] = advect(m_u[m_velocityIndex], btScalar(i), btScalar(j), u, v, dt);
    }
  }

  for (int i = 1; i < m_numNodesWidth - 1; i++)
  {
    for (int j = 1; j < m_numNodesLength - 1; j++)
    {
      int index = arrayIndex(i, j);
      m_u[m_velocityIndex][index] = m_temp[index];
    }
  }
}

void btHfFluid::advectVelocityV(btScalar dt)
{
  for (int i = 1; i < m_numNodesWidth - 1; i++)
  {
    for (int j = 1; j < m_numNodesLength - 1; j++)
    {
      int index = arrayIndex(i, j);
      if (!m_flags[index])
      {
        continue;
      }
      btScalar u = m_globalVelocityU;
      btScalar v = m_globalVelocityV;

      u += (m_u[m_velocityIndex][index] + m_u[m_velocityIndex][index + 1]
          + m_u[m_velocityIndex][index + m_numNodesWidth] + m_u[m_velocityIndex][index + m_numNodesWidth + 1])
          * btScalar(0.25);
      v += m_v[m_velocityIndex][index];

      m_temp[index] = advect(m_v[m_velocityIndex], btScalar(i), btScalar(j), u, v, dt);
    }
  }
  for (int i = 1; i < m_numNodesWidth - 1; i++)
  {
    for (int j = 1; j < m_numNodesLength - 1; j++)
    {
      int index = arrayIndex(i, j);
      m_v[m_velocityIndex][index] = m_temp[index];
    }
  }
}

void btHfFluid::addDisplaced(int i, int j, btScalar r)
{
  m_r[m_rIndex][arrayIndex(i, j)] += r;
}

void btHfFluid::transferDisplaced(btScalar dt)
{
  for (int i = 2; i < m_numNodesWidth - 2; i++)
  {
    for (int j = 2; j < m_numNodesLength - 2; j++)
    {
      btScalar deltaR = m_r[m_rIndex][arrayIndex(i, j)] - m_r[(m_rIndex + 1) % 2][arrayIndex(i, j)];
      // deltaR is in volume, but we want to change the height..
      deltaR = deltaR / (m_gridCellWidth * m_gridCellWidth);
      deltaR *= m_volumeDisplacementScale;
      btScalar qdeltaR = deltaR / btScalar(4.0f);
      m_eta[arrayIndex(i - 1, j - 1)] += qdeltaR;
      m_eta[arrayIndex(i - 1, j + 1)] += qdeltaR;
      m_eta[arrayIndex(i + 1, j - 1)] += qdeltaR;
      m_eta[arrayIndex(i + 1, j + 1)] += qdeltaR;
      m_eta[arrayIndex(i, j)] -= deltaR;
      // OPTIMIZATION: zero out next frames r value
      m_r[(m_rIndex + 1) % 2][arrayIndex(i, j)] = btScalar(0.0);
    }
  }
  m_rIndex = (m_rIndex + 1) % 2; // flip frame
}

void btHfFluid::updateVelocity(btScalar dt)
{
  for (int j = 1; j < m_numNodesLength - 1; j++)
  {
    for (int i = 2; i < m_numNodesWidth - 1; i++)
    {
      int index = arrayIndex(i, j);
      if (!m_flags[index])
      {
        continue;
      }
      m_u[m_velocityIndex][index] += m_gravity * dt * m_gridCellWidthInv
          * (m_height[m_heightIndex][index] - m_height[m_heightIndex][index - 1]);
    }
  }

  for (int j = 2; j < m_numNodesLength - 1; j++)
  {
    for (int i = 1; i < m_numNodesWidth - 1; i++)
    {
      int index = arrayIndex(i, j);
      if (!m_flags[index])
      {
        continue;
      }
      m_v[m_velocityIndex][index] += m_gravity * dt * m_gridCellWidthInv
          * (m_height[m_heightIndex][index] - m_height[m_heightIndex][index - m_numNodesWidth]);
    }
  }
}

void btHfFluid::setReflectBoundaryLeft()
{
  for (int j = 0; j < m_numNodesLength; j++)
  {
    int indexL = arrayIndex(0, j);

    m_height[m_heightIndex][indexL] = m_height[m_heightIndex][indexL + 1];
    m_u[m_velocityIndex][indexL + 1] = btScalar(0.0);
    m_v[m_velocityIndex][indexL] = btScalar(0.0);
  }
}

void btHfFluid::setReflectBoundaryRight()
{
  for (int j = 0; j < m_numNodesLength; j++)
  {
    int indexR = arrayIndex(m_numNodesWidth - 1, j);

    m_height[m_heightIndex][indexR] = m_height[m_heightIndex][indexR - 1];
    m_u[m_velocityIndex][indexR - 1] = btScalar(0.0);
    m_v[m_velocityIndex][indexR] = btScalar(0.0);
  }
}

void btHfFluid::setReflectBoundaryBottom()
{
  for (int i = 0; i < m_numNodesWidth; i++)
  {
    int indexT = arrayIndex(i, 0);

    m_height[m_heightIndex][indexT] = m_height[m_heightIndex][indexT + m_numNodesWidth];
    m_v[m_velocityIndex][indexT + m_numNodesWidth] = btScalar(0.0);
    m_u[m_velocityIndex][indexT] = btScalar(0.0);
  }
}

void btHfFluid::setReflectBoundaryTop()
{
  for (int i = 0; i < m_numNodesWidth; i++)
  {
    int indexB = arrayIndex(i, m_numNodesLength - 1);

    m_height[m_heightIndex][indexB] = m_height[m_heightIndex][indexB - m_numNodesWidth];
    m_v[m_velocityIndex][indexB - m_numNodesWidth] = btScalar(0.0);
    m_u[m_velocityIndex][indexB] = btScalar(0.0);
  }
}

void btHfFluid::setAbsorbBoundaryLeft(btScalar dt)
{
  for (int j = 0; j < m_numNodesLength; j++)
  {
    int indexL = arrayIndex(0, j);

    btScalar c = btSqrt(m_eta[indexL + 1] * m_gravity);
    m_height[m_heightIndex][indexL] = ((m_gridCellWidthInv * m_height[(m_heightIndex + 1) % 2][indexL + 1])
        + (dt * c * m_height[m_heightIndex][indexL + 1])) / (m_gridCellWidthInv + dt * c);
    m_u[m_velocityIndex][indexL + 1] = btScalar(0.0f);
    m_v[m_velocityIndex][indexL + 1] = btScalar(0.0);
  }
}

void btHfFluid::setAbsorbBoundaryRight(btScalar dt)
{
}

void btHfFluid::setAbsorbBoundaryTop(btScalar dt)
{
}

void btHfFluid::setAbsorbBoundaryBottom(btScalar dt)
{
}

void btHfFluid::computeFlagsAndFillRatio()
{
  for (int i = 1; i < m_numNodesWidth - 1; i++)
  {
    for (int j = 1; j < m_numNodesLength - 1; j++)
    {
      btScalar h = m_height[m_heightIndex][arrayIndex(i, j)];
      btScalar hMin = computeHmin(i, j);
      btScalar hMax = computeHmax(i, j);
      btScalar etaMax = computeEtaMax(i, j);
      if (h <= hMin && etaMax < m_epsEta)
      {
        m_flags[arrayIndex(i, j)] = false;
        m_fillRatio[arrayIndex(i, j)] = btScalar(0.0f);
      }
      else if (h > hMax)
      {
        m_flags[arrayIndex(i, j)] = true;
        m_fillRatio[arrayIndex(i, j)] = btScalar(1.0f);
      }
      else
      {
        m_flags[arrayIndex(i, j)] = true;
        m_fillRatio[arrayIndex(i, j)] = (h - hMin) / (hMax - hMin);
      }

    }
  }
}

btScalar btHfFluid::computeHmin(int i, int j)
{
  btAssert(i > 0);
  btAssert(i < m_numNodesWidth - 1);
  btAssert(j > 0);
  btAssert(j < m_numNodesLength - 1);

  btScalar h1 = m_height[m_heightIndex][arrayIndex(i - 1, j - 1)];
  btScalar h2 = m_height[m_heightIndex][arrayIndex(i - 1, j + 1)];
  btScalar h3 = m_height[m_heightIndex][arrayIndex(i + 1, j - 1)];
  btScalar h4 = m_height[m_heightIndex][arrayIndex(i + 1, j + 1)];
  btScalar h = m_height[m_heightIndex][arrayIndex(i, j)];
  btScalar minh = btMin(h1, btMin(h2, btMin(h3, h4)));

  return (minh + h) * btScalar(0.5f);
}

btScalar btHfFluid::computeHmax(int i, int j)
{
  btAssert(i > 0);
  btAssert(i < m_numNodesWidth - 1);
  btAssert(j > 0);
  btAssert(j < m_numNodesLength - 1);

  btScalar h1 = m_height[m_heightIndex][arrayIndex(i - 1, j - 1)];
  btScalar h2 = m_height[m_heightIndex][arrayIndex(i - 1, j + 1)];
  btScalar h3 = m_height[m_heightIndex][arrayIndex(i + 1, j - 1)];
  btScalar h4 = m_height[m_heightIndex][arrayIndex(i + 1, j + 1)];
  btScalar h = m_height[m_heightIndex][arrayIndex(i, j)];
  btScalar maxh = btMax(h1, btMax(h2, btMax(h3, h4)));

  return (maxh + h) * btScalar(0.5f) + m_epsHeight;
}

btScalar btHfFluid::computeEtaMax(int i, int j)
{
  btAssert(i > 0);
  btAssert(i < m_numNodesWidth - 1);
  btAssert(j > 0);
  btAssert(j < m_numNodesLength - 1);

  btScalar eta1 = m_eta[arrayIndex(i - 1, j - 1)];
  btScalar eta2 = m_eta[arrayIndex(i - 1, j + 1)];
  btScalar eta3 = m_eta[arrayIndex(i + 1, j - 1)];
  btScalar eta4 = m_eta[arrayIndex(i + 1, j + 1)];
  btScalar eta = m_eta[arrayIndex(i, j)];
  btScalar maxeta = btMax(eta1, btMax(eta2, btMax(eta3, eta4)));

  return (maxeta + eta) * btScalar(0.5f);
}

void btHfFluid::allocateArrays()
{
  if (m_temp)
    btAlignedFree (m_temp);
  if (m_height[0])
  {
    btAlignedFree (m_height[0]);
    btAlignedFree(m_height[1]);
  }
  if (m_ground)
    btAlignedFree (m_ground);
  if (m_eta)
    btAlignedFree (m_eta);
  if (m_u[0])
  {
    btAlignedFree (m_u[0]);
    btAlignedFree(m_u[1]);
  }
  if (m_v)
  {
    btAlignedFree (m_v[0]);
    btAlignedFree(m_v[1]);
  }
  if (m_r)
  {
    btAlignedFree (m_r[0]);
    btAlignedFree(m_r[1]);
  }
  if (m_flags)
    btAlignedFree (m_flags);
  if (m_fillRatio)
    btAlignedFree (m_fillRatio);

  m_heightIndex = 0;
  m_velocityIndex = 0;
  m_temp = (btScalar*)btAlignedAlloc(sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
  m_height[0] = (btScalar*)btAlignedAlloc(sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
  m_height[1] = (btScalar*)btAlignedAlloc(sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
  m_ground = (btScalar*)btAlignedAlloc(sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
  m_eta = (btScalar*)btAlignedAlloc(sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
  m_u[0] = (btScalar*)btAlignedAlloc(sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
  m_u[1] = (btScalar*)btAlignedAlloc(sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
  m_v[0] = (btScalar*)btAlignedAlloc(sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
  m_v[1] = (btScalar*)btAlignedAlloc(sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
  m_r[0] = (btScalar*)btAlignedAlloc(sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
  m_r[1] = (btScalar*)btAlignedAlloc(sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
  m_fillRatio = (btScalar*)btAlignedAlloc(sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
  m_flags = (bool*)btAlignedAlloc(sizeof(bool) * m_numNodesWidth * m_numNodesLength, 16);

  {
    int bufferSize = sizeof(btScalar) * m_numNodesWidth * m_numNodesLength;
    /*printf("[HfFluid] Fluid buffer size %d bytes\n", bufferSize);
     printf("[HfFluid] Temp %d buffers\n", 1);
     printf("[HfFluid] Height %d buffers\n", 2);
     printf("[HfFluid] Velocity %d buffers\n", 4);
     printf("[HfFluid] Eta %d buffers\n", 1);
     printf("[HfFluid] Fill Ratio %d buffers\n", 1);
     printf("[HfFluid] ===============================\n");
     printf("[HfFluid] Total %d buffers\n", 9);
     printf("[HfFluid] Total %d KB\n", (9 * bufferSize)/1024);*/
  }
  for (int i = 0; i < m_numNodesWidth * m_numNodesLength; i++)
  {
    m_eta[i] = btScalar(0.0);
    m_u[0][i] = btScalar(0.0);
    m_u[1][i] = btScalar(0.0);
    m_v[0][i] = btScalar(0.0);
    m_v[1][i] = btScalar(0.0);
    m_r[0][i] = btScalar(0.0);
    m_r[1][i] = btScalar(0.0);
    m_height[0][i] = btScalar(0.0);
    m_height[1][i] = btScalar(0.0);
    m_ground[i] = btScalar(-10.0);
    m_flags[i] = false;
    m_fillRatio[i] = btScalar(0.0);
    m_temp[i] = btScalar(0.0);
  }
}

void btHfFluid::debugTests()
{
  static btScalar total_volume = btScalar(0.0f);
  btScalar new_total_volume = btScalar(0.0f);
  for (int i = 0; i < m_numNodesWidth * m_numNodesLength; i++)
  {
    new_total_volume += m_eta[i] * m_gridCellWidth * m_gridCellWidth;
  }
  //printf("volume = %f volume delta = %f\n", new_total_volume, new_total_volume - total_volume);
  total_volume = new_total_volume;
}

// You can enforce a global velocity at the surface of the fluid
// default: 0.0 and 0.0
void btHfFluid::setGlobaVelocity(btScalar globalVelocityU, btScalar globalVelocityV)
{
  m_globalVelocityU = globalVelocityU;
  m_globalVelocityV = globalVelocityV;
}

void btHfFluid::getGlobalVelocity(btScalar& globalVelocityU, btScalar& globalVelocityV) const
{
  globalVelocityU = m_globalVelocityU;
  globalVelocityV = m_globalVelocityV;
}

// Control force of gravity, should match physics world
// default: -10.0
void btHfFluid::setGravity(btScalar gravity)
{
  m_gravity = gravity;
}
btScalar btHfFluid::getGravity() const
{
  return m_gravity;
}

// When a body is submerged into the fluid, the displaced fluid
// is spread to adjacent cells. You can control the percentage of this
// by setting this value between 0.0 and 1.0
// default: 0.5
void btHfFluid::setVolumeDisplacementScale(btScalar volumeDisplacementScale)
{
  m_volumeDisplacementScale = volumeDisplacementScale;
}

btScalar btHfFluid::getVolumeDisplacementScale() const
{
  return m_volumeDisplacementScale;
}

// The horizontal velocity of the fluid can influence bodies submerged
// in the fluid. You can control how much influence by setting this
// between 0.0 and 1.0
// default: 0.5
void btHfFluid::setHorizontalVelocityScale(btScalar horizontalVelocityScale)
{
  m_horizontalVelocityScale = horizontalVelocityScale;
}

btScalar btHfFluid::getHorizontalVelocityScale() const
{
  return m_horizontalVelocityScale;
}

static btScalar rangeOverlap(btScalar lo1, btScalar hi1, btScalar lo2, btScalar hi2, btScalar& loOut, btScalar& hiOut)
{
  if (!(lo1 <= hi2 && lo2 <= hi1))
    return btScalar(0.0f);

  if (lo1 >= lo2 && lo1 <= hi2 && hi1 >= lo2 && hi1 <= hi2)
  {
    hiOut = hi1;
    loOut = lo1;
    return hi1 - lo1;
  }
  else if (lo2 >= lo1 && lo2 <= hi1 && hi2 >= lo1 && hi2 <= hi1)
  {
    hiOut = hi2;
    loOut = lo2;
    return hi2 - lo2;
  }
  else if (hi1 >= lo2 && lo1 <= lo2)
  {
    hiOut = hi1;
    loOut = lo2;
    return hi1 - lo2;
  }
  else
  {
    hiOut = hi2;
    loOut = lo1;
    return hi2 - lo1;
  }
}

btHfFluidColumnRigidBodyCallback::btHfFluidColumnRigidBodyCallback(btRigidBody* rigidBody, btIDebugDraw* debugDraw,
                                                                   btScalar density, btScalar floatyness)
{
  m_rigidBody = rigidBody;
  m_buoyantShape = (btHfFluidBuoyantConvexShape*)rigidBody->getCollisionShape();
  m_debugDraw = debugDraw;
  m_rigidBody->getAabb(m_aabbMin, m_aabbMax);
  m_volume = btScalar(0.0f);
  m_density = density;
  m_floatyness = floatyness;
  m_numVoxels = m_buoyantShape->getNumVoxels();
  m_voxelPositionsXformed = (btVector3*)btAlignedAlloc(sizeof(btVector3) * m_numVoxels, 16);
  m_voxelSubmerged = (bool*)btAlignedAlloc(sizeof(bool) * m_numVoxels, 16);
  for (int i = 0; i < m_numVoxels; i++)
  {
    btVector3 p = m_buoyantShape->getVoxelPositionsArray()[i];
    p = m_rigidBody->getWorldTransform().getBasis() * p;
    p += m_rigidBody->getWorldTransform().getOrigin();
    m_voxelPositionsXformed[i] = p;
    m_voxelSubmerged[i] = false;
  }
}

btHfFluidColumnRigidBodyCallback::~btHfFluidColumnRigidBodyCallback()
{
  if (m_voxelPositionsXformed)
    btAlignedFree (m_voxelPositionsXformed);
  if (m_voxelSubmerged)
    btAlignedFree (m_voxelSubmerged);
}

static bool sphereVsAABB(const btVector3& aabbMin, const btVector3& aabbMax, const btVector3& sphereCenter,
                         const btScalar sphereRadius)
{
  btScalar totalDistance = 0;

  // Accumulate the distance of the sphere's center on each axis
  for (int i = 0; i < 3; ++i)
  {

    // If the sphere's center is outside the aabb, we've got distance on this axis
    if (sphereCenter[i] < aabbMin[i])
    {
      btScalar borderDistance = aabbMin[i] - sphereCenter[i];
      totalDistance += borderDistance * borderDistance;

    }
    else if (sphereCenter[i] > aabbMax[i])
    {
      btScalar borderDistance = sphereCenter[i] - aabbMax[i];
      totalDistance += borderDistance * borderDistance;

    }
    // Otherwise the sphere's center is within the box on this axis, so the
    // distance will be 0 and we do not need to accumulate anything at all

  }

  // If the distance to the box is lower than the sphere's radius, both are overlapping
  return (totalDistance <= (sphereRadius * sphereRadius));
}

bool btHfFluidColumnRigidBodyCallback::processColumn(btHfFluid* fluid, int w, int l)
{
  btVector3 columnAabbMin, columnAabbMax;

  fluid->getAabbForColumn(w, l, columnAabbMin, columnAabbMax);

  bool applyBuoyancyImpulse = true;
  bool applyFluidVelocityImpulse = true;
  bool applyFluidDisplace = true;

  btScalar dt = btScalar(1.0f / 60.0f);

  btScalar columnVolume = btScalar(0.0f);

  for (int i = 0; i < m_buoyantShape->getNumVoxels(); i++)
  {
    if (m_voxelSubmerged[i])
      continue;

    if (sphereVsAABB(columnAabbMin, columnAabbMax, m_voxelPositionsXformed[i], m_buoyantShape->getVoxelRadius()))
    {

      m_voxelSubmerged[i] = true;
      btScalar voxelVolume = m_buoyantShape->getVolumePerVoxel();
      columnVolume += voxelVolume;

      btVector3 application_point = m_voxelPositionsXformed[i];
      btVector3 relative_position = application_point - m_rigidBody->getCenterOfMassPosition();

      if (applyBuoyancyImpulse)
      {
        //btScalar massDisplacedWater = voxelVolume * m_density * m_floatyness;  <-- this density is object density, changed to water density(constant)
        btScalar massDisplacedWater = voxelVolume * 1027 * m_floatyness;
        btScalar force = massDisplacedWater * -fluid->getGravity();
        btScalar impulseMag = force * dt;
        //std::cout<<"Aquin: "<<impulseMag<<std::endl;
        btVector3 impulseVec = btVector3(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f)) * impulseMag;
        //std::cout<<"Aquin: "<<impulseVec.x()<<" "<<impulseVec.y()<<" "<<impulseVec.z()<<" "<<std::endl;
//#define CENTER_IMPULSE_ONLY 1
#ifdef CENTER_IMPULSE_ONLY
        m_rigidBody->applyCentralImpulse (impulseVec);
#else
        m_rigidBody->applyImpulse(impulseVec, relative_position);
#endif
      }
    }
  }

  if (columnVolume > btScalar(0.0))
  {
    m_volume += columnVolume;

    if (applyFluidDisplace)
    {
      fluid->addDisplaced(w, l, columnVolume);
    }

    if (applyFluidVelocityImpulse)
    {
      int index = fluid->arrayIndex(w, l);
      btScalar u = fluid->getVelocityUArray()[index];
      btScalar v = fluid->getVelocityVArray()[index];
      btVector3 vd = btVector3(u, btScalar(0.0f), v);
      btVector3 impulse = vd * dt * fluid->getHorizontalVelocityScale();
      //std::cout<<"Aquin: "<<impulse.x()<<" "<<impulse.y()<<" "<<impulse.z()<<" "<<std::endl;
      m_rigidBody->applyCentralImpulse(impulse);
    }
  }

  return true;
}