kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Base: new Rotation constructor - on vectors

Browse Source 
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())
This commit is contained in:
DeepSOIC
2017-09-02 03:42:04 +03:00
committed by wmayer
parent 8284e60401
commit 62b0d35270
4 changed files with 194 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
src/Base/Rotation.cpp
View File

@@ -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<Vector3d*> 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

18
src/Base/Rotation.h
View File

@@ -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];

4
src/Base/RotationPy.xml
View File

@@ -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).
</UserDocu>
</Documentation>
<Methode Name="invert">

27
src/Base/RotationPyImp.cpp
View File

@@ -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;
}