Initial commit

Lts/Proxies/Buildings/BuildingControlUnit/VWM/vwm_deposition.cpp

@@ -0,0 +1,122 @@
+/**
+  * @RDE_HEADER@
+  * Deposition calculation
+  * $Id$
+  * $HeadURL$
+  * $Author$
+  * Created by Gerlinde Rudolphus, SLSA, 2013-03-05
+  */
+
+
+#include "vwm_deposition.h"
+#include "vwm_math.h"
+#include <cmath>
+
+namespace BuildingControlUnit
+{
+namespace VWM
+{
+
+static double getDistPositionCode(char chDeposition)
+{
+  double dist = 0.0;
+  unsigned char absDeposition = static_cast<unsigned char>(chDeposition);
+
+  if (absDeposition > 127)
+    absDeposition = 256 - absDeposition;
+
+  if (absDeposition < 28)
+    dist = 0.1 * absDeposition;
+  else if (absDeposition < 96)
+    dist = exp(absDeposition / 27.1828);
+  else
+  {
+    dist = 0.0;
+    // ERROR_LOG(m_logger) << "ERROR: Invalid BSM-deposition";
+  }
+
+  if (static_cast<unsigned char>(chDeposition) > 127)
+    dist = -dist;
+
+  return dist;
+}
+
+static double getAngle(double x, double y)
+{
+
+  if (qFuzzyIsNull(x) && qFuzzyIsNull(y))
+    return 0.0;
+
+  double phi;
+  if (!qFuzzyIsNull(x))
+  {
+    phi = atan(y / x);
+    if (x < 0)
+    {
+      if (y > 0)
+        phi += M_PI;
+      else
+        phi -= M_PI;
+    }
+  }
+  else
+  {
+    phi = M_PI / 2.0;
+    if (y < 0.0)
+      phi = -phi;
+  }
+
+  return phi;
+}
+
+
+LtsUtils::QPoint3D getDeposition(int deposition1, int deposition2, const LtsUtils::QPoint3D &bsmDirection)
+{
+  LtsUtils::QPoint3D ret;
+  // keine Ablage -> return
+  if (deposition1 == 0 && deposition2 == 0)
+    return ret;
+
+  const double alpha = 45.0;
+  const double dyAlpha = cos(alpha * M_PI / 180.0);
+  const double dzAlpha = sin(alpha * M_PI / 180.0);
+
+  const double dy1 =  dyAlpha * getDistPositionCode(deposition1);
+  const double dz1 = -dzAlpha * getDistPositionCode(deposition1);
+  const double dy2 = -dyAlpha * getDistPositionCode(deposition2);
+  const double dz2 = -dzAlpha * getDistPositionCode(deposition2);
+
+  ret.setY(dy1 + dy2);
+  ret.setZ(dz1 + dz2);
+
+  const double x = bsmDirection.x();
+  const double y = bsmDirection.y();
+  const double z = bsmDirection.z();
+
+  const double phi = getAngle(x, y);
+
+  if (qFuzzyIsNull(z))
+  {
+    //    INFO_LOG(m_logger) << "phi_1=" << 180.0 * phi / PI;
+    ret = Math::rotateAroundZAxis(ret, -phi);
+  }
+  else
+  {
+    const double theta = getAngle(sqrt(x * x + y * y), z);
+    //    INFO_LOG(m_logger) << "phi_2=" << 180.0 * phi / PI << ", theta=" << 180.0 * theta / PI;
+    //    INFO_LOG(m_logger) << "pos0 = (" << posBsmDepositon.getPosition3DX()
+    //                       << "," << posBsmDepositon.getPosition3DY()
+    //                       << "," << posBsmDepositon.getPosition3DZ() << ")";
+    ret = Math::rotateAroundYAxis(ret, theta);
+    //     INFO_LOG(m_logger) << "pos1 = (" << pos1.getPosition3DX()
+    //                        << "," << pos1.getPosition3DY()
+    //                        << "," << pos1.getPosition3DZ() << ")";
+    ret = Math::rotateAroundZAxis(ret, -phi);
+  }
+
+  return ret;
+
+}
+
+} // namespace VWM
+} // namespace BuildingControlUnit