From 96f3f0fa26382bdbf7dbfa48798816cde9aacc0b Mon Sep 17 00:00:00 2001 From: DeepSOIC Date: Sat, 2 Sep 2017 03:42:04 +0300 Subject: [PATCH] Base: new Rotation constructor - on vectors The new constructor accepts wanted directions of x,y,z axes of rotated frame, and a priority string that affects how the vectors are made perpendicular to each other. Example - construct placement for sketch on XZ plane: v = App.Vector App.Rotation(v(1,0,0),v(0,0,1),v()) --- src/Base/Rotation.cpp | 145 +++++++++++++++++++++++++++++++++++++ src/Base/Rotation.h | 18 +++++ src/Base/RotationPy.xml | 4 + src/Base/RotationPyImp.cpp | 27 +++++++ 4 files changed, 194 insertions(+) diff --git a/src/Base/Rotation.cpp b/src/Base/Rotation.cpp index 77dc8f4a68..c87f45a696 100644 --- a/src/Base/Rotation.cpp +++ b/src/Base/Rotation.cpp @@ -29,6 +29,7 @@ #include "Rotation.h" #include "Matrix.h" +#include "Base/Exception.h" using namespace Base; @@ -386,6 +387,150 @@ Rotation Rotation::identity(void) return Rotation(0.0, 0.0, 0.0, 1.0); } +Rotation Rotation::makeRotationByAxes(Vector3d xdir, Vector3d ydir, Vector3d zdir, const char* priorityOrder) +{ + const double tol = 1e-7; //equal to OCC Precision::Confusion + enum dirIndex { + X, + Y, + Z + }; + + //convert priorityOrder string into a sequence of ints. + if(strlen(priorityOrder)!=3) + THROWM(ValueError, "makeRotationByAxes: length of priorityOrder is not 3"); + int order[3]; + for(int i = 0; i < 3; ++i){ + order[i] = priorityOrder[i] - 'X'; + if (order[i] < 0 || order[i] > 2) + THROWM(ValueError, "makeRotationByAxes: characters in priorityOrder must be uppercase X, Y, or Z. Some other character encountered.") + } + + //ensure every axis is listed in priority list + if( order[0] == order[1] || + order[1] == order[2] || + order[2] == order[0]) + THROWM(ValueError,"makeRotationByAxes: not all axes are listed in priorityOrder"); + + + //group up dirs into an array, to access them by indexes stored in @order. + std::vector dirs = {&xdir, &ydir, &zdir}; + + + auto dropPriority = [&order](int index){ + char tmp; + if (index == 0){ + tmp = order[0]; + order[0] = order[1]; + order[1] = order[2]; + order[2] = tmp; + } else if (index == 1) { + tmp = order[1]; + order[1] = order[2]; + order[2] = tmp; + } //else if index == 2 do nothing + }; + + //pick up the strict direction + Vector3d mainDir; + for(int i = 0; i < 3; ++i){ + mainDir = *(dirs[order[0]]); + if (mainDir.Length() > tol) + break; + else + dropPriority(0); + if (i == 2) + THROWM(ValueError, "makeRotationByAxes: all directions supplied are zero"); + } + mainDir.Normalize(); + + //pick up the 2nd priority direction, "hint" direction. + Vector3d hintDir; + for(int i = 0; i < 2; ++i){ + hintDir = *(dirs[order[1]]); + if ((hintDir.Cross(mainDir)).Length() > tol) + break; + else + dropPriority(1); + if (i == 1) + hintDir = Vector3d(); //no vector can be used as hint direction. Zero it out, to indicate that a guess is needed. + } + if (hintDir.Length() == 0){ + switch (order[0]){ + case X: { //xdir is main + //align zdir to OZ + order[1] = Z; + order[2] = Y; + hintDir = Vector3d(0,0,1); + if ((hintDir.Cross(mainDir)).Length() <= tol){ + //aligning to OZ is impossible, align to ydir to OY. Why so? I don't know, just feels right =) + hintDir = Vector3d(0,1,0); + order[1] = Y; + order[2] = Z; + } + } break; + case Y: { //ydir is main + //align zdir to OZ + order[1] = Z; + order[2] = X; + hintDir = mainDir.z > -tol ? Vector3d(0,0,1) : Vector3d(0,0,-1); + if ((hintDir.Cross(mainDir)).Length() <= tol){ + //aligning zdir to OZ is impossible, align xdir to OX then. + hintDir = Vector3d(1,0,0); + order[1] = X; + order[2] = Z; + } + } break; + case Z: { //zdir is main + //align ydir to OZ + order[1] = Y; + order[2] = X; + hintDir = Vector3d(0,0,1); + if ((hintDir.Cross(mainDir)).Length() <= tol){ + //aligning ydir to OZ is impossible, align xdir to OX then. + hintDir = Vector3d(1,0,0); + order[1] = X; + order[2] = Y; + } + } break; + }//switch ordet[0] + } + + //ensure every axis is listed in priority list + assert(order[0] != order[1]); + assert(order[1] != order[2]); + assert(order[2] != order[0]); + + hintDir.Normalize(); + //make hintDir perpendicular to mainDir. For that, we cross-product the two to obtain the third axis direction, and then recover back the hint axis by doing another cross product. + Vector3d lastDir = mainDir.Cross(hintDir); + lastDir.Normalize(); + hintDir = lastDir.Cross(mainDir); + hintDir.Normalize(); //redundant? + + Vector3d finaldirs[3]; + finaldirs[order[0]] = mainDir; + finaldirs[order[1]] = hintDir; + finaldirs[order[2]] = lastDir; + + //fix handedness + if (finaldirs[X].Cross(finaldirs[Y]) * finaldirs[Z] < 0.0) + //handedness is wrong. Switch the direction of the least important axis + finaldirs[order[2]] = finaldirs[order[2]] * (-1.0); + + //build the rotation, by constructing a matrix first. + Matrix4D m; + m.setToUnity(); + for(int i = 0; i < 3; ++i){ + //matrix indexing: [row][col] + m[0][i] = finaldirs[i].x; + m[1][i] = finaldirs[i].y; + m[2][i] = finaldirs[i].z; + } + + return Rotation(m); +} + void Rotation::setYawPitchRoll(double y, double p, double r) { // The Euler angles (yaw,pitch,roll) are in XY'Z''-notation diff --git a/src/Base/Rotation.h b/src/Base/Rotation.h index 7ded2c7231..e38f239fe4 100644 --- a/src/Base/Rotation.h +++ b/src/Base/Rotation.h @@ -82,9 +82,27 @@ public: bool isSame(const Rotation&) const; //@} + /** Specialty constructors */ static Rotation slerp(const Rotation & rot0, const Rotation & rot1, double t); static Rotation identity(void); + /** + * @brief makeRotationByAxes(xdir, ydir, zdir, priorityOrder): creates a rotation + * that converts a vector in local cs with axes given as arguments, into a + * vector in global cs. + * @param xdir is wanted direction of local X axis + * @param ydir ... + * @param zdir + * @param priorityOrder sets which directions are followed. It is a string + * like "ZXY". This means, Z direction is followed precisely; X direction is + * corrected to be perpendicular to Z direction, and used; Y direction + * argument is ignored altogether (Y direction is generated from Z and X). + * + * If only one vector provided is nonzero, the other two directions are picked automatically. + */ + static Rotation makeRotationByAxes(Vector3d xdir, Vector3d ydir, Vector3d zdir, const char* priorityOrder = "ZXY"); + + private: void normalize(); double quat[4]; diff --git a/src/Base/RotationPy.xml b/src/Base/RotationPy.xml index 42a64d142a..9d8a010e09 100644 --- a/src/Base/RotationPy.xml +++ b/src/Base/RotationPy.xml @@ -25,6 +25,10 @@ -- three floats (Euler angles) as yaw-pitch-roll in XY'Z'' convention -- four floats (Quaternion) where the quaternion is specified as: q=xi+yj+zk+w, i.e. the last parameter is the real part + -- three vectors that define rotated axes directions + an optional + 3-characher string of capital letters 'X', 'Y', 'Z' that sets the + order of importance of the axes (e.g., 'ZXY' means z direction is + followed strictly, x is used but corrected if necessary, y is ignored). diff --git a/src/Base/RotationPyImp.cpp b/src/Base/RotationPyImp.cpp index 65cd3849aa..5eac5abf92 100644 --- a/src/Base/RotationPyImp.cpp +++ b/src/Base/RotationPyImp.cpp @@ -150,6 +150,32 @@ int RotationPy::PyInit(PyObject* args, PyObject* /*kwd*/) return 0; } + PyErr_Clear(); + PyObject *v3; + char *priority = nullptr; + if (PyArg_ParseTuple(args, "O!O!O!|s", &(Base::VectorPy::Type), &v1, + &(Base::VectorPy::Type), &v2, + &(Base::VectorPy::Type), &v3, + &priority )) { + Py::Vector xdir(v1, false); + Py::Vector ydir(v2, false); + Py::Vector zdir(v3, false); + if (!priority) + priority = "ZXY"; + try { + *getRotationPtr() = (Rotation::makeRotationByAxes(xdir.toVector(), ydir.toVector(), zdir.toVector(), priority)); + } catch(Base::Exception &e) { + std::string str; + str += "FreeCAD exception thrown ("; + str += e.what(); + str += ")"; + PyErr_SetString(Base::BaseExceptionFreeCADError,str.c_str()); + return -1; + } + + return 0; + } + PyErr_SetString(PyExc_TypeError, "Rotation constructor accepts:\n" "-- empty parameter list\n" "-- Rotation object" @@ -160,6 +186,7 @@ int RotationPy::PyInit(PyObject* args, PyObject* /*kwd*/) "-- Matrix object\n" "-- 16 floats (4x4 matrix)\n" "-- 9 floats (3x3 matrix)\n" + "-- 3 vectors + optional string" ); return -1; }