DualQuaternion.h

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (c) 2019 Viktor Titov (DeepSOIC) <vv.titov@gmail.com>       *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  is distributed in the hope that it will be useful,      *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

#ifndef FREECAD_BASE_DUAL_QUATERNION_H
#define FREECAD_BASE_DUAL_QUATERNION_H

#include "DualNumber.h"
//#include <Console.h> //DEBUG

namespace Base {

class LX_BASE_EXPORT DualQuat {
public:
    DualNumber x;
    DualNumber y;
    DualNumber z;
    DualNumber w;
public:
    DualQuat(){}
    DualQuat(DualNumber x, DualNumber y, DualNumber z, DualNumber w)
        : x(x), y(y), z(z), w(w) {}
    DualQuat(double x,double y,double z,double w,double dx,double dy,double dz,double dw)
        : x(x, dx), y(y, dy), z(z, dz), w(w, dw) {}
    DualQuat(double x,double y,double z,double w)
        : x(x), y(y), z(z), w(w) {}

    DualQuat(DualQuat re, DualQuat du);

    static DualQuat identity() {return DualQuat(0.0, 0.0, 0.0, 1.0);}

    DualQuat real() const {return DualQuat(x.re, y.re, z.re, w.re);}

    DualQuat dual() const {return DualQuat(x.du, y.du, z.du, w.du);}

    DualQuat conj() const {return DualQuat(-x, -y, -z, w);}

    DualQuat vec() const {return DualQuat(x,y,z,0.0);}

    double length() const {return sqrt(x.re*x.re + y.re*y.re + z.re*z.re + w.re*w.re);}

    double theta() const {return 2.0 * atan2(vec().length(), w.re);}

    static double dot(DualQuat a, DualQuat b);

    DualQuat pow(double t, bool shorten = true) const;

    DualQuat operator-() const {return DualQuat(-x, -y, -z, -w);}

    //DEBUG
    //void print() const {
    //    Console().Log("%f, %f, %f, %f; %f, %f, %f, %f", x.re,y.re,z.re,w.re, x.du,y.du,z.du, w.du);
    //}
};

DualQuat operator+(DualQuat a, DualQuat b);
DualQuat operator-(DualQuat a, DualQuat b);
DualQuat operator*(DualQuat a, DualQuat b);

DualQuat operator*(DualQuat a, double b);
DualQuat operator*(double a, DualQuat b);
DualQuat operator*(DualQuat a, DualNumber b);
DualQuat operator*(DualNumber a, DualQuat b);


} //namespace

#endif