MathTools.cpp

Go to the documentation of this file.

// *****************************************************************
// Filename:    AspectGraphProcessor.cpp
// Copyright:   Kai Welke, Chair Prof. Dillmann (IAIM),
//              Institute for Computer Science and Engineering (CSE),
//              University of Karlsruhe. All rights reserved.
// Author:      Kai Welke
// Date:        12.06.2007
// *****************************************************************
 
// *****************************************************************
// includes
// *****************************************************************
#include "MathTools.h"
#include <cmath>
//#include "Base/Tools/DebugMemory.h"
 
 
// **********************************************************
// transformation on sphere coordinates and paths
// **********************************************************
// returns transformation from p1 -> p2
TTransform MathTools::determinePathTransformation(TPath* p1, TPath* p2)
{
    TSphereCoord start1, start2, end1, end2;
    start1 = p1->at(0).Position;
    end1 = p1->at(p1->size() - 1).Position;
    start2 = p2->at(0).Position;
    end2 = p2->at(p2->size() - 1).Position;
 
    return determineTransformation(start1, start2, end1, end2);
}
 
TPath* MathTools::transformPath(TTransform transform, TPath* pPath)
{
    TPath* pResulting = new TPath();
 
    if (pPath->size() < 2)
    {
        printf("ERROR: path is too short\n");
        delete pResulting;
        return nullptr;
    }
 
    // copy path and transform
    for (auto element : *pPath)
    {
        TSphereCoord res = MathTools::transform(element.Position, transform);
        element.Position = res;
 
        pResulting->push_back(element);
    }
 
    return pResulting;
 
}
 
TPath* MathTools::inverseTransformPath(TTransform transform, TPath* pPath)
{
    TPath* pResulting = new TPath();
 
    if (pPath->size() < 2)
    {
        printf("ERROR: path is too short\n");
        delete pResulting;
        return nullptr;
    }
 
    // copy path and transform
    for (auto element : *pPath)
    {
        TSphereCoord res = MathTools::inverseTransform(element.Position, transform);
        element.Position = res;
 
        pResulting->push_back(element);
    }
 
    return pResulting;
 
}
 
TTransform MathTools::determineTransformation(TSphereCoord start1, TSphereCoord start2, TSphereCoord end1, TSphereCoord end2)
{
    TTransform result;
 
    // first determine alpha and beta and beta axis
    float fAlpha, fBeta;
    Eigen::Vector3d betaAxis;
    determineTransformationAlphaBeta(start1, start2, fAlpha, fBeta, betaAxis);
    result.fAlpha = fAlpha;
    result.fBeta = fBeta;
    result.betaAxis = betaAxis;
 
    // calculate angular for psi rotation
 
    // apply transform to first path end
    TSphereCoord end1_transformed = transformAlphaBeta(end1, fAlpha, fBeta, betaAxis);
 
    Eigen::Vector3d end1_transformed_v, start2_v, end2_v;
    convert(end1_transformed, end1_transformed_v);
    convert(start2, start2_v);
    convert(end2, end2_v);
 
    // calculate angle between start2 <-> end1_tranformed ; start2 <-> end2
    Eigen::Vector3d zero = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d line1 = MathTools::dropAPerpendicular(end1_transformed_v, zero, start2_v);
    Eigen::Vector3d line2 = MathTools::dropAPerpendicular(end2_v, zero, start2_v);
    line1 -= end1_transformed_v;
    line2 -= end2_v;
 
    float fPsi = MathTools::AngleAndDirection(line1, line2, start2_v) * 180.0 / M_PI;
 
    result.fPsi = fPsi;
    result.psiAxis = start2_v;
 
    return result;
}
 
TSphereCoord MathTools::transform(TSphereCoord sc, TTransform transform)
{
    // first apply alpha , beta
    TSphereCoord rotated_ab = transformAlphaBeta(sc, transform.fAlpha, transform.fBeta, transform.betaAxis);
 
    Eigen::Vector3d rotated_ab_v;
 
    convert(rotated_ab, rotated_ab_v);
 
    // now apply final rotation
    Eigen::AngleAxisd rot(-transform.fPsi * M_PI / 180.0, transform.psiAxis);
    rotated_ab_v = rot.matrix() * rotated_ab_v;
 
    TSphereCoord rotated;
    convert(rotated_ab_v, rotated);
 
    return rotated;
}
 
TSphereCoord MathTools::inverseTransform(TSphereCoord sc, TTransform transform)
{
    Eigen::Vector3d sc_v;
    convert(sc, sc_v);
 
    // first rotate
    Eigen::AngleAxisd rot(transform.fPsi * M_PI / 180.0,  transform.psiAxis);
    sc_v = rot.matrix() * sc_v;
 
    TSphereCoord rotated;
    convert(sc_v, rotated);
 
    return inverseTransformAlphaBeta(rotated, transform.fAlpha, transform.fBeta, transform.betaAxis);
}
 
// determine alpha and beta of a transform
// fAlpha: rotation around the z axis in counterclockwise direction considering the positive z-axis
// fBeta: rotation around the rotated y axis in counterclockwise direction considering the positive y-axis
void MathTools::determineTransformationAlphaBeta(TSphereCoord sc1, TSphereCoord sc2, float& fAlpha, float& fBeta, Eigen::Vector3d& betaAxis)
{
    Eigen::Vector3d vector1, vector2;
    convert(sc1, vector1);
    convert(sc2, vector2);
 
    // project both vectors to xy plane
    Eigen::Vector3d vector1_xy, vector2_xy;
 
    vector1_xy = vector1;
    vector2_xy = vector2;
 
    vector1_xy(2) = 0.0;
    vector2_xy(2) = 0.0;
 
    // calculate first angle from vec1_projected to vec2_projected with normale z
    Eigen::Vector3d normal = Eigen::Vector3d::UnitZ();
 
    if (vector1_xy.norm() == 0.0 || vector2_xy.norm() == 0)
    {
        fAlpha = ((sc2.fTheta - sc1.fTheta) / 180.0 * M_PI);
    }
    else
    {
        fAlpha = MathTools::AngleAndDirection(vector1_xy, vector2_xy, normal);
    }
 
    // now rotate first vector with alpha
    Eigen::Vector3d rotated1, rotated_y;
 
    // the vector to rotate
 
    rotated1 = vector1;
    // set rotation: alpha around z-axis to transform v1 in same plane as v2
 
    Eigen::AngleAxisd rotation(fAlpha, Eigen::Vector3d::UnitZ());;
    Eigen::AngleAxisd rotation_yaxis(sc2.fTheta, Eigen::Vector3d::UnitZ());
 
    // we need normal for angle calculateion, so also rotate x-axis
    rotated_y << 0.0, 1.0, 0.0;
 
    // rotate vector and x-axis
 
 
    rotated1 = rotation.matrix() * rotated1;
 
    rotated_y =  rotation_yaxis.matrix() * rotated_y;
 
    betaAxis = rotated_y;
 
    // calculate angle between both vectors
    fBeta = MathTools::AngleAndDirection(rotated1, vector2, rotated_y);
 
    fAlpha *= 180.0 / M_PI;
    fBeta *= 180.0 / M_PI;
}
 
TSphereCoord MathTools::transformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis)
{
    TSphereCoord result;
    Eigen::Vector3d vector1, vector1_rotated;
    convert(sc, vector1);
 
    // rotate around positive z-axis
    Eigen::AngleAxisd rotation_alpha(fAlpha  / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
 
    // rotate vector aroun z axis
    vector1_rotated = rotation_alpha.matrix() * vector1;
    // rotate vector with beta around betaAxis
    Eigen::AngleAxisd rot(-fBeta / 180.0 * M_PI, betaAxis);
 
    vector1_rotated = rot.matrix() * vector1_rotated;
 
    convert(vector1_rotated, result);
 
    return result;
}
 
TSphereCoord MathTools::inverseTransformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis)
{
    Eigen::Vector3d vector1;
    convert(sc, vector1);
 
    // do inverse of beta axis rotation
    Eigen::AngleAxisd rot(fBeta / 180.0 * M_PI, betaAxis);
    vector1 = rot.matrix() * vector1;
 
    // do inverse of zaxis rotation
    Eigen::AngleAxisd rotation_alpha(-fAlpha / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
 
    // rotate vector aroun z axis
    vector1 = rotation_alpha * vector1;
 
    TSphereCoord result;
    convert(vector1, result);
 
    return result;
}
 
// **********************************************************
// calculations on sphere coordinates
// **********************************************************
float MathTools::CalculateCrossProductFromDifference(TSphereCoord p1, TSphereCoord p2, TSphereCoord p3)
{
    float u1, v1, u2, v2;
 
    u1 =  p2.fPhi - p1.fPhi;
    v1 =  p2.fTheta - p1.fTheta;
    u2 =  p3.fPhi - p1.fPhi;
    v2 =  p3.fTheta - p1.fTheta;
 
    return u1 * v2 - v1 * u2;
}
 
double MathTools::Angle(Eigen::Vector3d vec1, Eigen::Vector3d vec2)
{
    double r = vec1.dot(vec2) / (vec1.norm() * vec2.norm());
    r = std::min(1.0, std::max(-1.0, r));
    return  std::acos(r);
}
 
float MathTools::getDistanceOnArc(TSphereCoord p1, TSphereCoord p2)
{
    Eigen::Vector3d vec1, vec2;
    p1.fDistance = 1.0f;
    p2.fDistance = 1.0f;
 
    convert(p1, vec1);
    convert(p2, vec2);
 
    float fAngle = (float) MathTools::Angle(vec1, vec2);
 
    // length of circular arc
    // not necessary because Math3d::Angle returns a value in radians
    //float fDist = float( (2 * M_PI * fAngle ) / 360.0f);
 
    return fAngle;
}
 
float MathTools::getDistance(TSphereCoord p1, TSphereCoord p2, float fRadius)
{
    // never use this!!!!
    TSphereCoord diff;
    diff.fPhi = p1.fPhi - p2.fPhi;
    diff.fTheta = p1.fTheta - p2.fTheta;
 
    float fAngle = sqrt(diff.fPhi * diff.fPhi + diff.fTheta * diff.fTheta);
 
    return fAngle;
}
 
// **********************************************************
// extended 2d Math
// **********************************************************
bool MathTools::IntersectLines(Eigen::Vector2d p1, Eigen::Vector2d m1, Eigen::Vector2d p2, Eigen::Vector2d m2, Eigen::Vector2d& res)
{
    // calculate perpendicular bisector of sides
    float dx = float(p1(0) - p2(0));
    float dy = float(p1(1) - p2(1));
 
    // now from
    // g1: g1(x,y) = (p1x,p1y) + (m1x,m1y) * c1
    // g2: g2(x,y) = (p2x,p2y) + (m2x,m2y) * c2
    // -->
    // g1(x,y) = g2(x,y)
    // -->
    // (p1x,p1y) + (m1x,m1y) * c1 = (p2x,p2y) + (m2x,m2y) * c2
    // (p1x,p1y) - (p2x,p2y) = (dx,dy) = (m2x,m2y) * c2 - (m1x,m1y) * c1
    // -->
    // dx = m2x * c2 - m1x * c1,  dy = m2y * c2 - m1y * c1
    // c1 = (m2y * c2 - dy) / m1y
    // dx = m2x * c2 - m1x * (m2y * c2 - dy) / m1y
 
    float z = float(dx - (m1(0) * dy) / m1(1));
    float n = float(m2(0) - (m1(0) * m2(1)) / m1(1));
 
    float c2;
 
    if (n == 0)
    {
        return false;
    }
    else
    {
        c2 = z / n;
    }
 
    res(0) = p2(0) + c2 * m2(0);
    res(1) = p2(1) + c2 * m2(1);
 
    return true;
}
 
// **********************************************************
// extended 3d Math
// **********************************************************
// checked and working
float MathTools::AngleAndDirection(Eigen::Vector3d vector1, Eigen::Vector3d vector2, Eigen::Vector3d normal)
{
    double fAngle = MathTools::Angle(vector1, vector2);
 
    Eigen::Vector3d axis = vector1.cross(vector2);
 
    // compare axis and normal
    normal.normalize();
 
    double c =   normal.dot(vector1);
 
    axis += vector1;
    double d = normal.dot(axis) - c;
 
 
    if (d < 0)
    {
        return 2 * M_PI - fAngle;
    }
 
    return (float) fAngle;
}
 
// checked and working
Eigen::Vector3d MathTools::dropAPerpendicular(Eigen::Vector3d point, Eigen::Vector3d linepoint, Eigen::Vector3d linevector)
{
    // helping plane normal (ax + by + cz + d = 0)
    Eigen::Vector3d plane_normal = linevector; // (a,b,c)
    float d = (float) - plane_normal.dot(point);
 
    // calculate k from plane and line intersection
    float z = float(- plane_normal(0) * linepoint(0) - plane_normal(1) * linepoint(1) - plane_normal(2) * linepoint(2) - d);
    float n = float(plane_normal(0) * linevector(0) + plane_normal(1) * linevector(1) + plane_normal(2) * linevector(2));
 
    float k = z / n;
 
    // calculate line from line equation
    linevector = k * linevector;
    linevector += linepoint;
 
    return linevector;
 
}
 
// **********************************************************
// conversions
// **********************************************************
// checked and working
void MathTools::convert(TSphereCoord in, Eigen::Vector3d& out)
{
    // alpha is azimuth, beta is polar angle
    in.fPhi *= M_PI / 180.0f;
    in.fTheta *= M_PI / 180.0f;
 
    // OLD and wrong??
    //  out.x = in.fDistance * sin(in.fPhi) * cos(in.fTheta);
    //  out.y = in.fDistance * sin(in.fPhi) * sin(in.fTheta);
    //  out.z = in.fDistance * cos(in.fPhi);
 
    Eigen::Vector3d vector = Eigen::Vector3d::UnitZ();
    Eigen::AngleAxisd rotation_y(in.fPhi, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd rotation_z(in.fTheta, Eigen::Vector3d::UnitZ());
 
 
    // first rotate around y axis with phi
    vector = rotation_y.matrix() * vector;
 
    // second rotate around z with theta
    vector = rotation_z.matrix() * vector;
 
    out = vector;
}
 
 
void MathTools::convertWithDistance(TSphereCoord in, Eigen::Vector3d& out)
{
    convert(in, out);
    out *= in.fDistance;
}
 
void MathTools::convert(TSphereCoord in, Eigen::Vector3f& out)
{
    Eigen::Vector3d vec;
    convert(in, vec);
    out = vec.cast<float>();
}
 
 
// checked and working
void MathTools::convert(Eigen::Vector3d in, TSphereCoord& out)
{
    // alpha is azimuth, beta is polar angle
    out.fDistance = in.norm();
 
    float fAngle = in(2) / out.fDistance;
 
    if (fAngle < -1.0f)
    {
        fAngle = -1.0f;
    }
 
    if (fAngle > 1.0f)
    {
        fAngle = 1.0f;
    }
 
    out.fPhi = (float) acos(fAngle);
 
    out.fTheta = (float) atan2(in(1), in(0));
 
    // convert to angles and correct interval
    out.fPhi /= M_PI / 180.0f;
    out.fTheta /= M_PI / 180.0f;
 
    // only positive rotations
    if (out.fTheta < 0.0)
    {
        out.fTheta = 360.0f + out.fTheta;
    }
 
    // be aware of limit
    if (out.fPhi > 180.0)
    {
        out.fPhi = out.fPhi - 180.0f;
    }
}
 
 
void MathTools::convert(Eigen::Vector3f in, TSphereCoord& out)
{
    Eigen::Vector3d vec(in(0), in(1), in(2));
    convert(vec, out);
}