123 lines
2.8 KiB
C++
Executable File
123 lines
2.8 KiB
C++
Executable File
/**
|
|
* @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
|