BeamUtils.cpp

#include <cassert>
#include <numeric>
#include "BeamUtils.h"
#include <glm/gtx/quaternion.hpp>
 
#include "Context.h"
#include "Resources/MeshManager.h"
 
using glm::quat;
using glm::vec3;
using glm::vec4;
using glm::sin;
using glm::sqrt;
using glm::cross;
using glm::normalize;
using glm::angleAxis;
using glm::radians;
using glm::dot;
using glm::min;
using glm::max;
using glm::mat4;
using namespace Cogs::Core;
 
namespace {
 
  // Using Cramer's rule to solve the three simultaneous equations.
  // No check for degeneracy.
  glm::vec3 triplePlaneIntersection(const glm::vec4& eq1,
                                    const glm::vec4& eq2,
                                    const glm::vec4& eq3)
  {
 
    auto A = glm::determinant(glm::mat3(eq1.x, eq2.x, eq3.x,
                                        eq1.y, eq2.y, eq3.y,
                                        eq1.z, eq2.z, eq3.z));
 
    auto A1 = glm::determinant(glm::mat3(-eq1.w, -eq2.w, -eq3.w,
                                         eq1.y, eq2.y, eq3.y,
                                         eq1.z, eq2.z, eq3.z));
 
    auto A2 = glm::determinant(glm::mat3(eq1.x, eq2.x, eq3.x,
                                         -eq1.w, -eq2.w, -eq3.w,
                                         eq1.z, eq2.z, eq3.z));
 
    auto A3 = glm::determinant(glm::mat3(eq1.x, eq2.x, eq3.x,
                                         eq1.y, eq2.y, eq3.y,
                                         -eq1.w, -eq2.w, -eq3.w));
 
    return (1.f / A)*glm::vec3(A1, A2, A3);
  }
 
}
 
void EchoSounder::getBoundingFrustum(vec4* planes,
                                     const glm::quat& rotation,
                                     const glm::vec3& translation,
                                     const uint32_t coordSys,
                                     const float minDirX, const float maxDirX,
                                     const float minDirY, const float maxDirY,
                                     const float depthMin, const float depthMax)
{
  auto d00 = rotation * getBeamDir(coordSys, minDirX, minDirY);
  auto d01 = rotation * getBeamDir(coordSys, maxDirX, minDirY);
  auto d10 = rotation * getBeamDir(coordSys, minDirX, maxDirY);
  auto d11 = rotation * getBeamDir(coordSys, maxDirX, maxDirY);
 
  auto avg = normalize(d00 + d01 + d10 + d11);
 
  vec3 n[4]{
    normalize(cross(d00, d01)),
    normalize(cross(d11, d10)),
    normalize(cross(d01, d11)),
    normalize(cross(d10, d00)),
  };
 
  auto b = min(min(dot(d00, avg), dot(d10, avg)),
               min(dot(d01, avg), dot(d11, avg)));
 
  planes[0] = vec4(n[0], -dot(n[0], translation));
  planes[1] = vec4(n[1], -dot(n[1], translation));
  planes[2] = vec4(n[2], -dot(n[2], translation));
  planes[3] = vec4(n[3], -dot(n[3], translation));
  planes[4] = vec4(avg,  -b*depthMin - dot(avg, translation));
  planes[5] = vec4(-avg, depthMax + dot(avg, translation));
 
  // Planes points towards inside:
  //auto m = translation + 0.5f*(depthMax + depthMin)*avg;
  //for (int i = 0; i < 6; i++) {
  //  assert( dot(planes[i], vec4(m, 1.f)) > 0.f);
  //}
}
 
 
void EchoSounder::getAxisAlignedBoundingBox(glm::vec3& minCorner,
                                            glm::vec3& maxCorner,
                                            const glm::quat& rotation,
                                            const glm::vec3& translation,
                                            const uint32_t coordSys,
                                            const float minDirX, const float maxDirX,
                                            const float minDirY, const float maxDirY,
                                            const float depthMin, const float depthMax)
{
  if (coordSys == 0) {
    auto sx = sin(minDirX);
    auto sy = sin(minDirY);
    float r = glm::sqrt(1.f - sx*sx - sy*sy);
 
    vec3 _min = depthMin*(rotation * vec3(sx, sy, r));
    vec3 _max = _min;
 
    for (uint32_t j = 0; j < 25; j++) {
      auto v = (1.f / 24.f)*j;
 
      for (uint32_t i = 0; i < 25; i++) {
        auto u = (1.f / 24.f)*i;
        auto sx_ = sin((1.f - u)*minDirX + u*maxDirX);
        auto sy_ = sin((1.f - v)*minDirY + v*maxDirY);
        float r_ = glm::sqrt(1.f - sx_*sx_ - sy_*sy_);
        auto p = (rotation * vec3(sx_, sy_, r_));
        auto a = depthMin*p;
        auto b = depthMax*p;
        _min = min(_min, min(a, b));
        _max = max(_max, max(a, b));
      }
    }
 
    minCorner = _min + translation;
    maxCorner = _max + translation;
  }
  else {
    assert(false && "FIXME");
  }
}
 
vec3 EchoSounder::getBeamDir(uint32_t coordSys, float dirX, float dirY)
{
  vec3 rv;
  switch (coordSys) {
  case 0:
  {
    // Echo sounder coordsys (EK80, ME70, MS70)
    auto sx = sin(dirX);
    auto sy = sin(dirY);
    float r = glm::sqrt(1.f - sx*sx - sy*sy);
    rv = vec3(sx, sy, r);
    break;
  }
  case 1:
  {
    // SN90 coordsys
    auto cx = cos(dirX);
    auto cy = cos(dirY);
    auto sx = sin(dirX);
    auto sy = sin(dirY);
    rv = vec3(sx, sy*cx, cx*cy);
    break;
  }
  case 2:
  { // ST90 coordsys
    auto cx = cos(dirX);
    auto cy = cos(dirY);
    auto sx = sin(dirX);
    auto sy = sin(dirY);
    rv = vec3(cx*cy, cx*sy, -sx);
    break;
  }
  default:
    assert(false && "unrecognized coordinate system");
    break;
  }
 
  return rv;
}
 
bool EchoSounder::isXDominant(const std::vector<float> &directionX, const std::vector<float> &directionY)
{
  return std::abs(directionY.back() - directionY.front()) < std::abs(directionX.back() - directionX.front());
}
 
 
void EchoSounder::getArrayToVesselTransform(quat& rotation,
                                            vec3& translation,
                                            const vec3& alpha,
                                            const vec3& offset)
{
  auto aX = angleAxis(alpha.x, vec3(1, 0, 0));
  auto aY = angleAxis(alpha.y, vec3(0, 1, 0));
  auto aZ = angleAxis(alpha.z, vec3(0, 0, 1));
  auto a = aZ * aY * aX;
  // We're now in SIMRAD vessel coords, X is towards bow, Y is towards
  // starboard, Z is downwards.
 
 
  // In TD50 coords, vessel is heading north, X is easting, Y is northing,
  // and Z is upwards.
  auto bX = angleAxis(radians(180.f), vec3(1, 0, 0)); // Rotate s.t. Z is up
  auto bZ = angleAxis(radians(-90.f), vec3(0, 0, 1));  // Rotate s.t. old X becomes Y.
  auto b = bX * bZ;
 
  rotation = b * a;
  translation = b * offset;
}
 
 
 
void EchoSounder::inflateFans(std::vector<float>& directionX,
                              std::vector<float>& directionY,
                              const std::vector<float>& inDirectionX,
                              const std::vector<float>& inBeamWidthX,
                              const std::vector<float>& inDirectionY,
                              const std::vector<float>& inBeamWidthY,
                              const std::vector<uint32_t>& beams,
                              const Topology /*topology*/,
                              const size_t minorCount)
{
  const size_t majorCount = beams.size() / minorCount;
  if (beams.size() == 1) {
    const auto srcIx = beams[0];
    directionX.resize(4);
    directionY.resize(4);
    directionX[0] = inDirectionX[srcIx] - 0.5f*inBeamWidthX[srcIx];
    directionY[0] = inDirectionY[srcIx] - 0.5f*inBeamWidthY[srcIx];
    directionX[1] = inDirectionX[srcIx] - 0.5f*inBeamWidthX[srcIx];
    directionY[1] = inDirectionY[srcIx] + 0.5f*inBeamWidthY[srcIx];
    directionX[2] = inDirectionX[srcIx] + 0.5f*inBeamWidthX[srcIx];
    directionY[2] = inDirectionY[srcIx] - 0.5f*inBeamWidthY[srcIx];
    directionX[3] = inDirectionX[srcIx] + 0.5f*inBeamWidthX[srcIx];
    directionY[3] = inDirectionY[srcIx] + 0.5f*inBeamWidthY[srcIx];
  }
  else if (majorCount == 1 || minorCount == 1) {
    size_t largestDimensionCount = std::max(majorCount, minorCount);
    bool xDom = isXDominant(inDirectionX, inDirectionY);
    directionX.resize(2 * largestDimensionCount);
    directionY.resize(2 * largestDimensionCount);
    for (size_t i = 0; i < largestDimensionCount; i++) {
      const auto srcIx = beams[i];
      directionX[i] = inDirectionX[srcIx] - (xDom ? 0.0f : 0.5f)*inBeamWidthX[srcIx];
      directionY[i] = inDirectionY[srcIx] - (xDom ? 0.5f : 0.f)*inBeamWidthY[srcIx];
      directionX[i + minorCount] = inDirectionX[srcIx] + (xDom ? 0.f : 0.5f)*inBeamWidthX[srcIx];
      directionY[i + minorCount] = inDirectionY[srcIx] + (xDom ? 0.5f : 0.f)*inBeamWidthY[srcIx];
    }
  }
  //if marjorCount > 1 and minorCount > 1 there is not need to inflate the fan, it's alreday a grid
}
 
 
MeshHandle EchoSounder::buildBeamBundleOutline(Context* context,
                                               const glm::quat& rotation,
                                               const glm::vec3& translation,
                                               const uint32_t coordSys,
                                               const float* dirX, const size_t nX,
                                               const float* dirY, const size_t nY,
                                               const float depthMin, const float depthMax)
{
  auto minorCount = nX;
  auto majorCount = nY;
 
  auto mesh = context->meshManager->create();
 
  auto P = mesh->mapPositions(0, 2 * minorCount*majorCount);
  auto C = mesh->map<glm::vec4>(VertexDataType::Colors0, VertexFormats::Color4f, 0, 2 * minorCount*majorCount);
 
 
  std::vector<uint32_t> indices;
  indices.reserve(8 * majorCount*minorCount);
  for (size_t j = 0; j < majorCount; j++) {
    for (size_t i = 0; i < minorCount; i++) {
      float v = i / (minorCount - 1.f);
      float u = j / (majorCount - 1.f);
 
      auto d = rotation *getBeamDir(coordSys, dirX[j], dirY[i]);
 
      //auto d = orientation * beamDirection(dataData->beamAngleAthwartship[i],
      //                                     dataData->beamAngleAlongship[j]);
 
      P[2 * (minorCount*j + i) + 0] = translation + depthMin*d;
      P[2 * (minorCount*j + i) + 1] = translation + depthMax*d;
      C[2 * (minorCount*j + i) + 0] = glm::vec4(u, v, 0.f, 1.f);
      C[2 * (minorCount*j + i) + 1] = glm::vec4(u, v, 0.f, 1.f);
 
      if (0 < i) {
        indices.push_back(static_cast<uint32_t>(2 * (minorCount*j + i - 1) + 0));
        indices.push_back(static_cast<uint32_t>(2 * (minorCount*j + i) + 0));
        indices.push_back(static_cast<uint32_t>(2 * (minorCount*j + i - 1) + 1));
        indices.push_back(static_cast<uint32_t>(2 * (minorCount*j + i) + 1));
      }
      if (0 < j) {
        indices.push_back(static_cast<uint32_t>(2 * (minorCount*(j - 1) + i) + 0));
        indices.push_back(static_cast<uint32_t>(2 * (minorCount*j + i) + 0));
        indices.push_back(static_cast<uint32_t>(2 * (minorCount*(j - 1) + i) + 1));
        indices.push_back(static_cast<uint32_t>(2 * (minorCount*j + i) + 1));
      }
    }
  }
  indices.push_back(static_cast<uint32_t>(2 * (minorCount * 0 + 0) + 0));
  indices.push_back(static_cast<uint32_t>(2 * (minorCount * 0 + 0) + 1));
  indices.push_back(static_cast<uint32_t>(2 * (minorCount * 0 + minorCount - 1) + 0));
  indices.push_back(static_cast<uint32_t>(2 * (minorCount * 0 + minorCount - 1) + 1));
  indices.push_back(static_cast<uint32_t>(2 * (minorCount * (majorCount - 1) + 0) + 0));
  indices.push_back(static_cast<uint32_t>(2 * (minorCount * (majorCount - 1) + 0) + 1));
  indices.push_back(static_cast<uint32_t>(2 * (minorCount * (majorCount - 1) + minorCount - 1) + 0));
  indices.push_back(static_cast<uint32_t>(2 * (minorCount * (majorCount - 1) + minorCount - 1) + 1));
 
 
  mesh->setIndexData(indices.data(), indices.size());
  mesh->primitiveType = Cogs::PrimitiveType::LineList;
  mesh->setBounds(calculateBounds(mesh.operator->()));
 
  return mesh;
}
 
 
void EchoSounder::buildBeamBundleOutline2(std::vector<glm::vec3>& V,
                                          std::vector<uint32_t>& indices,
                                          Context* context,
                                          const glm::quat& rotation,
                                          const glm::vec3& translation,
                                          const uint32_t coordSys,
                                          const float* dirX, const size_t nX,
                                          const float* dirY, const size_t nY,
                                          const float depthMin, const float depthMax,
                                          const bool minorClosed,
                                          const bool individualBeams)
{
  auto minorCount = nY;
  auto majorCount = nX;
 
  size_t numDepthRings = 2;
  std::vector<float> depthRingValues;
  for (size_t k = 0; k < numDepthRings; ++k) {
    depthRingValues.push_back((depthMax - depthMin) * (k / (numDepthRings - 1.f)) + depthMin);
  }
 
  auto mesh = context->meshManager->create();
 
  for (uint32_t j = 0; j < majorCount; j++) {
    for (uint32_t i = 0; i < minorCount; i++) {
      auto d = rotation *getBeamDir(coordSys,
                                    dirX[minorCount*j + i],
                                    dirY[minorCount*j + i]);
 
      for (size_t k = 0; k < numDepthRings; ++k) {
        V.push_back(translation + depthRingValues[k] * d);
      }
 
      //line between vertices which has the same depthRing value across minor direction
      if (0 < i && (individualBeams || j == 0 || j == majorCount -1)) {
        for (size_t k = 0; k < numDepthRings; ++k) {
          indices.push_back(static_cast<uint32_t>(numDepthRings * (minorCount*j + i - 1) + k));
          indices.push_back(static_cast<uint32_t>(numDepthRings * (minorCount*j + i) + k));
        }
      }
 
      //line between vertices which has the same depthRing value across major direction
      if (!minorClosed) {
        if (0 < j && (individualBeams || i == 0 || i == minorCount - 1)) {
          for (size_t k = 0; k < numDepthRings; ++k) {
            indices.push_back(static_cast<uint32_t>(numDepthRings * (minorCount*(j - 1) + i) + k));
            indices.push_back(static_cast<uint32_t>(numDepthRings * (minorCount*j + i) + k));
          }
        }
      }
    }
  }
 
 
  if (!minorClosed) {
    // End caps
    indices.push_back(static_cast<uint32_t>(numDepthRings * (minorCount * 0 + 0) + 0));
    indices.push_back(static_cast<uint32_t>(numDepthRings * (minorCount * 0 + 0) + numDepthRings - 1));
    indices.push_back(static_cast<uint32_t>(numDepthRings * (minorCount * 0 + minorCount - 1) + 0));
    indices.push_back(static_cast<uint32_t>(numDepthRings * (minorCount * 0 + minorCount - 1) + numDepthRings - 1));
    indices.push_back(static_cast<uint32_t>(numDepthRings * (minorCount * (majorCount - 1) + 0) + 0));
    indices.push_back(static_cast<uint32_t>(numDepthRings * (minorCount * (majorCount - 1) + 0) + numDepthRings - 1));
    indices.push_back(static_cast<uint32_t>(numDepthRings * (minorCount * (majorCount - 1) + minorCount - 1) + 0));
    indices.push_back(static_cast<uint32_t>(numDepthRings * (minorCount * (majorCount - 1) + minorCount - 1) + numDepthRings - 1));
  }
}
 
MeshHandle EchoSounder::buildFrustumOutline(Context* context,
                                            const glm::vec4* planes)
{
  auto mesh = context->meshManager->create();
  auto P = mesh->mapPositions(0, 8);
  auto C = mesh->map<glm::vec4>(VertexDataType::Colors0, VertexFormats::Color4f, 0, 8);
 
  for (int i = 0; i < 8; i++) {
    P[i] = triplePlaneIntersection(planes[0 + ((i >> 0) & 1)],
                                   planes[2 + ((i >> 1) & 1)],
                                   planes[4 + ((i >> 2) & 1)]);
    C[i] = glm::vec4(0, 1, 1, 1);
  }
 
  std::vector<uint32_t> indices =
  {
    0, 1, 1, 3, 3, 2, 2, 0,
    4, 5, 5, 7, 7, 6, 6, 4,
    0, 4, 1, 5, 2, 6, 3, 7
  };
 
  mesh->setIndexData(indices.data(), indices.size());
  mesh->primitiveType = Cogs::PrimitiveType::LineList;
  mesh->setBounds(calculateBounds(mesh.operator->()));
 
  return mesh;
}
 
 
MeshHandle EchoSounder::buildBoxOutline(Context* context,
                                        const glm::vec3& minCorner,
                                        const glm::vec3& maxCorner)
{
  auto mesh = context->meshManager->create();
  auto P = mesh->mapPositions(0, 8);
  auto C = mesh->map<glm::vec4>(VertexDataType::Colors0, VertexFormats::Color4f, 0, 8);
 
  for (int i = 0; i < 8; i++) {
    P[i] = vec3(((i >> 0) & 1) == 0 ? minCorner.x : maxCorner.x,
      ((i >> 1) & 1) == 0 ? minCorner.y : maxCorner.y,
                ((i >> 2) & 1) == 0 ? minCorner.z : maxCorner.z);
    C[i] = glm::vec4(0, 0, 1, 1);
  }
 
  std::vector<uint32_t> indices =
  {
    0, 1, 1, 3, 3, 2, 2, 0,
    4, 5, 5, 7, 7, 6, 6, 4,
    0, 4, 1, 5, 2, 6, 3, 7
  };
 
  mesh->setIndexData(indices.data(), indices.size());
  mesh->primitiveType = Cogs::PrimitiveType::LineList;
  mesh->setBounds(calculateBounds(mesh.operator->()));
 
  return mesh;
}
 
void EchoSounder::clipVertical(Cogs::Memory::TypedBuffer<float> & values, size_t startValueIndex,
                               const glm::vec3& beamStartPos,
                               const std::vector<glm::vec3>& beamDir, const size_t startBeamIndex,
                               const float minVerticalDepth, const float maxVerticalDepth,
                               const float depthOffset, const float depthStep, const float maxRange,
                               const size_t beamCount, const size_t sampleCount)
{
 
  for (size_t j = 0; j < beamCount; ++j) {
    auto valueIndex = startValueIndex + j * sampleCount;
    auto & d = beamDir[j + startBeamIndex];
    auto & o = beamStartPos;
 
    size_t la;
    if (minVerticalDepth < 1.f) {
      la = 0;
    }
    else if (-maxRange * d.z < minVerticalDepth) {
      la = sampleCount;
    }
    else {
      float t0 = -(minVerticalDepth + o.z) / d.z;
      la = static_cast<uint32_t>(std::max(0.f, std::floor((t0 - depthOffset) / depthStep)));
    }
    la = std::min(la, sampleCount);
 
    size_t lb;
    if (maxVerticalDepth <= minVerticalDepth) {
      lb = la;
    }
    else if (-maxRange * d.z < minVerticalDepth) {
      lb = sampleCount;
    }
    else {
      float t0 = -(maxVerticalDepth + o.z) / d.z;
      lb = std::max(la, static_cast<size_t>(std::max(0.f, std::floor((t0 - depthOffset) / depthStep))));
    }
    lb = std::min(lb, sampleCount);
 
    auto out1 = valueIndex + 0;
    for (size_t i = 0; i < la; i++) {
      values[out1++] = 0.f;
    }
 
    auto out2 = valueIndex + lb;
    for (size_t i = lb; i < sampleCount; i++) {
      values[out2++] = 0.f;
    }
  }
}