void PropertyNormalList::transformGeometry(const Base::Matrix4D &mat)
{
// A normal vector is only a direction with unit length, so we only need to rotate it
// (no translations or scaling)
// Extract scale factors (assumes an orthogonal rotation matrix)
// Use the fact that the length of the row vectors of R are all equal to 1
// And that scaling is applied after rotating
double s[3];
s[0] = sqrt(mat[0][0] * mat[0][0] + mat[0][1] * mat[0][1] + mat[0][2] * mat[0][2]);
s[1] = sqrt(mat[1][0] * mat[1][0] + mat[1][1] * mat[1][1] + mat[1][2] * mat[1][2]);
s[2] = sqrt(mat[2][0] * mat[2][0] + mat[2][1] * mat[2][1] + mat[2][2] * mat[2][2]);
// Set up the rotation matrix: zero the translations and make the scale factors = 1
Base::Matrix4D rot;
rot.setToUnity();
for (unsigned short i = 0; i < 3; i++) {
for (unsigned short j = 0; j < 3; j++) {
rot[i][j] = mat[i][j] / s[i];
}
}
aboutToSetValue();
// Rotate the normal vectors
for (int ii=0; ii<getSize(); ii++) {
set1Value(ii, rot * operator[](ii));
}
hasSetValue();
}
void DraftDxfRead::OnReadInsert(const double* point, const double* scale, const char* name, double rotation)
{
std::cout << "Inserting block " << name << " rotation " << rotation << " pos " << point[0] << "," << point[1] << "," << point[2] << std::endl;
for(std::map<std::string,std::vector<Part::TopoShape*> > ::const_iterator i = layers.begin(); i != layers.end(); ++i) {
std::string k = i->first;
std::string prefix = "BLOCKS ";
prefix += name;
prefix += " ";
if(k.substr(0, prefix.size()) == prefix) {
BRep_Builder builder;
TopoDS_Compound comp;
builder.MakeCompound(comp);
std::vector<Part::TopoShape*> v = i->second;
for(std::vector<Part::TopoShape*>::const_iterator j = v.begin(); j != v.end(); ++j) {
const TopoDS_Shape& sh = (*j)->_Shape;
if (!sh.IsNull())
builder.Add(comp, sh);
}
if (!comp.IsNull()) {
Part::TopoShape* pcomp = new Part::TopoShape(comp);
Base::Matrix4D mat;
mat.scale(scale[0],scale[1],scale[2]);
mat.rotZ(rotation);
mat.move(point[0],point[1],point[2]);
pcomp->transformShape(mat,true);
AddObject(pcomp);
}
}
}
}
PyObject* MatrixPy::isOrthogonal(PyObject * args)
{
double eps=1.0e-06;
if (!PyArg_ParseTuple(args, "|d",&eps))
return 0;
const Base::Matrix4D& mat = *getMatrixPtr();
Base::Matrix4D trp = mat;
trp.transpose();
trp = trp * mat;
bool ok = true;
double mult = trp[0][0];
for (int i=0; i<4 && ok; i++) {
for (int j=0; j<4 && ok; j++) {
if (i != j) {
if (fabs(trp[i][j]) > eps) {
ok = false;
break;
}
}
else { // the main diagonal
if (fabs(trp[i][j]-mult) > eps) {
ok = false;
break;
}
}
}
}
return Py::new_reference_to(Py::Float(ok ? mult : 0.0));
}
App::DocumentObjectExecReturn *Prism::execute(void)
{
// Build a prism
if (Polygon.getValue() < 3)
return new App::DocumentObjectExecReturn("Polygon of prism is invalid, must have 3 or more sides");
if (Circumradius.getValue() < Precision::Confusion())
return new App::DocumentObjectExecReturn("Circumradius of the polygon, of the prism, is too small");
if (Height.getValue() < Precision::Confusion())
return new App::DocumentObjectExecReturn("Height of prism is too small");
try {
long nodes = Polygon.getValue();
Base::Matrix4D mat;
mat.rotZ(Base::toRadians(360.0/nodes));
// create polygon
BRepBuilderAPI_MakePolygon mkPoly;
Base::Vector3d v(Circumradius.getValue(),0,0);
for (long i=0; i<nodes; i++) {
mkPoly.Add(gp_Pnt(v.x,v.y,v.z));
v = mat * v;
}
mkPoly.Add(gp_Pnt(v.x,v.y,v.z));
BRepBuilderAPI_MakeFace mkFace(mkPoly.Wire());
BRepPrimAPI_MakePrism mkPrism(mkFace.Face(), gp_Vec(0,0,Height.getValue()));
this->Shape.setValue(mkPrism.Shape());
}
catch (Standard_Failure& e) {
return new App::DocumentObjectExecReturn(e.GetMessageString());
}
return Primitive::execute();
}
void PropertyCurvatureList::transformGeometry(const Base::Matrix4D &mat)
{
// The principal direction is only a vector with unit length, so we only need to rotate it
// (no translations or scaling)
// Extract scale factors (assumes an orthogonal rotation matrix)
// Use the fact that the length of the row vectors of R are all equal to 1
// And that scaling is applied after rotating
double s[3];
s[0] = sqrt(mat[0][0] * mat[0][0] + mat[0][1] * mat[0][1] + mat[0][2] * mat[0][2]);
s[1] = sqrt(mat[1][0] * mat[1][0] + mat[1][1] * mat[1][1] + mat[1][2] * mat[1][2]);
s[2] = sqrt(mat[2][0] * mat[2][0] + mat[2][1] * mat[2][1] + mat[2][2] * mat[2][2]);
// Set up the rotation matrix: zero the translations and make the scale factors = 1
Base::Matrix4D rot;
rot.setToUnity();
for (unsigned short i = 0; i < 3; i++) {
for (unsigned short j = 0; j < 3; j++) {
rot[i][j] = mat[i][j] / s[i];
}
}
aboutToSetValue();
// Rotate the principal directions
for (int ii=0; ii<getSize(); ii++)
{
CurvatureInfo ci = operator[](ii);
ci.cMaxCurvDir = rot * ci.cMaxCurvDir;
ci.cMinCurvDir = rot * ci.cMinCurvDir;
_lValueList[ii] = ci;
}
hasSetValue();
}
App::DocumentObjectExecReturn *RegularPolygon::execute(void)
{
// Build a regular polygon
if (Polygon.getValue() < 3)
return new App::DocumentObjectExecReturn("the polygon is invalid, must have 3 or more sides");
if (Circumradius.getValue() < Precision::Confusion())
return new App::DocumentObjectExecReturn("Circumradius of the polygon is too small");
try {
long nodes = Polygon.getValue();
Base::Matrix4D mat;
mat.rotZ(Base::toRadians(360.0/nodes));
// create polygon
BRepBuilderAPI_MakePolygon mkPoly;
Base::Vector3d v(Circumradius.getValue(),0,0);
for (long i=0; i<nodes; i++) {
mkPoly.Add(gp_Pnt(v.x,v.y,v.z));
v = mat * v;
}
mkPoly.Add(gp_Pnt(v.x,v.y,v.z));
this->Shape.setValue(mkPoly.Shape());
}
catch (Standard_Failure) {
Handle_Standard_Failure e = Standard_Failure::Caught();
return new App::DocumentObjectExecReturn(e->GetMessageString());
}
return App::DocumentObject::StdReturn;
}
App::DocumentObjectExecReturn *Prism::execute(void)
{
// Build a prism
if (Polygon.getValue() < 3)
return new App::DocumentObjectExecReturn("Polygon of prism is invalid");
if (Length.getValue() < Precision::Confusion())
return new App::DocumentObjectExecReturn("Radius of prism too small");
if (Height.getValue() < Precision::Confusion())
return new App::DocumentObjectExecReturn("Height of prism too small");
try {
long nodes = Polygon.getValue();
Base::Matrix4D mat;
mat.rotZ(Base::toRadians(360.0/nodes));
// create polygon
BRepBuilderAPI_MakePolygon mkPoly;
Base::Vector3d v(Length.getValue(),0,0);
for (long i=0; i<nodes; i++) {
mkPoly.Add(gp_Pnt(v.x,v.y,v.z));
v = mat * v;
}
mkPoly.Add(gp_Pnt(v.x,v.y,v.z));
BRepBuilderAPI_MakeFace mkFace(mkPoly.Wire());
BRepPrimAPI_MakePrism mkPrism(mkFace.Face(), gp_Vec(0,0,Height.getValue()));
this->Shape.setValue(mkPrism.Shape());
}
catch (Standard_Failure) {
Handle_Standard_Failure e = Standard_Failure::Caught();
return new App::DocumentObjectExecReturn(e->GetMessageString());
}
return App::DocumentObject::StdReturn;
}
// Create a Box and Place it a coords (x,y,z)
MeshObjectRef PlaceBox(float x, float y, float z)
{
MeshObjectRef mesh = new Mesh::MeshObject(*globalBox);
Base::Matrix4D m;
m.move(x,y,z);
mesh->getKernel().Transform(m);
return mesh;
}
PyObject* MatrixPy::transposed(PyObject * args)
{
if (!PyArg_ParseTuple(args, ""))
return NULL;
PY_TRY {
Base::Matrix4D m = *getMatrixPtr();
m.transpose();
return new MatrixPy(m);
}
PY_CATCH;
Py_Return;
}
PyObject* MeshPy::translate(PyObject *args)
{
float x,y,z;
if (!PyArg_ParseTuple(args, "fff",&x,&y,&z))
return NULL;
PY_TRY {
Base::Matrix4D m;
m.move(x,y,z);
getMeshObjectPtr()->getKernel().Transform(m);
} PY_CATCH;
Py_Return;
}
void PointKernel::transformGeometry(const Base::Matrix4D &rclMat)
{
std::vector<value_type>& kernel = getBasicPoints();
QtConcurrent::blockingMap(kernel, [rclMat](value_type& value) {
rclMat.multVec(value, value);
});
}
PyObject* MeshPy::rotate(PyObject *args)
{
double x,y,z;
if (!PyArg_ParseTuple(args, "ddd",&x,&y,&z))
return NULL;
PY_TRY {
Base::Matrix4D m;
m.rotX(x);
m.rotY(y);
m.rotZ(z);
getMeshObjectPtr()->getKernel().Transform(m);
} PY_CATCH;
Py_Return;
}
void TaskProjGroup::rotateButtonClicked(void)
{
if ( multiView && ui ) {
const QObject *clicked = sender();
// Any translation/scale/etc applied here will be ignored, as
// DrawProjGroup::setFrontViewOrientation() only
// uses it to set Direction and XAxisDirection.
Base::Matrix4D m = multiView->viewOrientationMatrix.getValue();
// TODO: Construct these directly
Base::Matrix4D t;
//TODO: Consider changing the vectors around depending on whether we're in First or Third angle mode - might be more intuitive? IR
if ( clicked == ui->butTopRotate ) {
t.rotX(M_PI / -2);
} else if ( clicked == ui->butCWRotate ) {
t.rotY(M_PI / -2);
} else if ( clicked == ui->butRightRotate) {
t.rotZ(M_PI / 2);
} else if ( clicked == ui->butDownRotate) {
t.rotX(M_PI / 2);
} else if ( clicked == ui->butLeftRotate) {
t.rotZ(M_PI / -2);
} else if ( clicked == ui->butCCWRotate) {
t.rotY(M_PI / 2);
}
m *= t;
multiView->setFrontViewOrientation(m);
Gui::Command::updateActive();
}
}
SbMatrix ViewProvider::convert(const Base::Matrix4D &rcMatrix) const
{
double dMtrx[16];
rcMatrix.getGLMatrix(dMtrx);
return SbMatrix(dMtrx[0], dMtrx[1], dMtrx[2], dMtrx[3],
dMtrx[4], dMtrx[5], dMtrx[6], dMtrx[7],
dMtrx[8], dMtrx[9], dMtrx[10], dMtrx[11],
dMtrx[12],dMtrx[13],dMtrx[14], dMtrx[15]);
}
void ViewProvider::setTransformation(const Base::Matrix4D &rcMatrix)
{
double dMtrx[16];
rcMatrix.getGLMatrix(dMtrx);
pcTransform->setMatrix(SbMatrix(dMtrx[0], dMtrx[1], dMtrx[2], dMtrx[3],
dMtrx[4], dMtrx[5], dMtrx[6], dMtrx[7],
dMtrx[8], dMtrx[9], dMtrx[10], dMtrx[11],
dMtrx[12],dMtrx[13],dMtrx[14], dMtrx[15]));
}
void Builder3D::addTransformation(const Base::Matrix4D& transform)
{
Base::Vector3f cAxis, cBase;
float fAngle, fTranslation;
transform.toAxisAngle(cBase, cAxis,fAngle,fTranslation);
cBase.x = (float)transform[0][3];
cBase.y = (float)transform[1][3];
cBase.z = (float)transform[2][3];
addTransformation(cBase,cAxis,fAngle);
}
void PropertyPointKernel::Restore(Base::XMLReader &reader)
{
reader.readElement("Points");
std::string file (reader.getAttribute("file") );
if (!file.empty()) {
// initate a file read
reader.addFile(file.c_str(),this);
}
if(reader.DocumentSchema > 3)
{
std::string Matrix (reader.getAttribute("mtrx") );
Base::Matrix4D mtrx;
mtrx.fromString(Matrix);
aboutToSetValue();
_cPoints->setTransform(mtrx);
hasSetValue();
}
}
PyObject* MatrixPy::inverse(PyObject * args)
{
if (!PyArg_ParseTuple(args, ""))
return NULL;
PY_TRY {
if (fabs(getMatrixPtr()->determinant()) > DBL_EPSILON) {
Base::Matrix4D m = *getMatrixPtr();
m.inverseGauss();
return new MatrixPy(m);
}
else {
PyErr_SetString(Base::BaseExceptionFreeCADError, "Cannot invert singular matrix");
return 0;
}
}
PY_CATCH;
Py_Return;
}
Base::Matrix4D AbstractPolygonTriangulator::GetTransformToFitPlane() const
{
PlaneFit planeFit;
for (std::vector<Base::Vector3f>::const_iterator it = _points.begin(); it!=_points.end(); ++it)
planeFit.AddPoint(*it);
if (planeFit.Fit() == FLOAT_MAX)
throw Base::Exception("Plane fit failed");
Base::Vector3f bs = planeFit.GetBase();
Base::Vector3f ex = planeFit.GetDirU();
Base::Vector3f ey = planeFit.GetDirV();
Base::Vector3f ez = planeFit.GetNormal();
// build the matrix for the inverse transformation
Base::Matrix4D rInverse;
rInverse.setToUnity();
rInverse[0][0] = ex.x; rInverse[0][1] = ey.x; rInverse[0][2] = ez.x; rInverse[0][3] = bs.x;
rInverse[1][0] = ex.y; rInverse[1][1] = ey.y; rInverse[1][2] = ez.y; rInverse[1][3] = bs.y;
rInverse[2][0] = ex.z; rInverse[2][1] = ey.z; rInverse[2][2] = ez.z; rInverse[2][3] = bs.z;
return rInverse;
}
void PropertyNormalList::transformGeometry(const Base::Matrix4D &mat)
{
// A normal vector is only a direction with unit length, so we only need to rotate it
// (no translations or scaling)
// Extract scale factors (assumes an orthogonal rotation matrix)
// Use the fact that the length of the row vectors of R are all equal to 1
// And that scaling is applied after rotating
double s[3];
s[0] = sqrt(mat[0][0] * mat[0][0] + mat[0][1] * mat[0][1] + mat[0][2] * mat[0][2]);
s[1] = sqrt(mat[1][0] * mat[1][0] + mat[1][1] * mat[1][1] + mat[1][2] * mat[1][2]);
s[2] = sqrt(mat[2][0] * mat[2][0] + mat[2][1] * mat[2][1] + mat[2][2] * mat[2][2]);
// Set up the rotation matrix: zero the translations and make the scale factors = 1
Base::Matrix4D rot;
rot.setToUnity();
for (unsigned short i = 0; i < 3; i++) {
for (unsigned short j = 0; j < 3; j++) {
rot[i][j] = mat[i][j] / s[i];
}
}
aboutToSetValue();
// Rotate the normal vectors
#ifdef _WIN32
Concurrency::parallel_for_each(_lValueList.begin(), _lValueList.end(), [rot](Base::Vector3f& value) {
value = rot * value;
});
#else
QtConcurrent::blockingMap(_lValueList, [rot](Base::Vector3f& value) {
rot.multVec(value, value);
});
#endif
hasSetValue();
}
// Analyse the a transformation Matrix and describe the transformation
std::string Matrix4D::analyse(void) const
{
const double eps=1.0e-06;
bool hastranslation = (dMtrx4D[0][3] != 0.0 ||
dMtrx4D[1][3] != 0.0 || dMtrx4D[2][3] != 0.0);
const Base::Matrix4D unityMatrix = Base::Matrix4D();
std::string text;
if (*this == unityMatrix)
{
text = "Unity Matrix";
}
else
{
if (dMtrx4D[3][0] != 0.0 || dMtrx4D[3][1] != 0.0 ||
dMtrx4D[3][2] != 0.0 || dMtrx4D[3][3] != 1.0)
{
text = "Projection";
}
else //translation and affine
{
if (dMtrx4D[0][1] == 0.0 && dMtrx4D[0][2] == 0.0 &&
dMtrx4D[1][0] == 0.0 && dMtrx4D[1][2] == 0.0 &&
dMtrx4D[2][0] == 0.0 && dMtrx4D[2][1] == 0.0) //scaling
{
std::ostringstream stringStream;
stringStream << "Scale [" << dMtrx4D[0][0] << ", " <<
dMtrx4D[1][1] << ", " << dMtrx4D[2][2] << "]";
text = stringStream.str();
}
else
{
Base::Matrix4D sub;
sub[0][0] = dMtrx4D[0][0]; sub[0][1] = dMtrx4D[0][1];
sub[0][2] = dMtrx4D[0][2]; sub[1][0] = dMtrx4D[1][0];
sub[1][1] = dMtrx4D[1][1]; sub[1][2] = dMtrx4D[1][2];
sub[2][0] = dMtrx4D[2][0]; sub[2][1] = dMtrx4D[2][1];
sub[2][2] = dMtrx4D[2][2];
Base::Matrix4D trp = sub;
trp.transpose();
trp = trp * sub;
bool ortho = true;
for (int i=0; i<4 && ortho; i++) {
for (int j=0; j<4 && ortho; j++) {
if (i != j) {
if (fabs(trp[i][j]) > eps) {
ortho = false;
break;
}
}
}
}
double determinant = sub.determinant();
if (ortho)
{
if (fabs(determinant-1.0)<eps ) //rotation
{
text = "Rotation Matrix";
}
else
{
if (fabs(determinant+1.0)<eps ) //rotation
{
text = "Rotinversion Matrix";
}
else //scaling with rotation
{
std::ostringstream stringStream;
stringStream << "Scale and Rotate ";
if (determinant<0.0 )
stringStream << "and Invert ";
stringStream << "[ " <<
sqrt(trp[0][0]) << ", " << sqrt(trp[1][1]) << ", " <<
sqrt(trp[2][2]) << "]";
text = stringStream.str();
}
}
}
else
{
std::ostringstream stringStream;
stringStream << "Affine with det= " <<
determinant;
text = stringStream.str();
}
}
}
if (hastranslation)
text += " with Translation";
}
return text;
}
void ComplexGeoData::applyTranslation(const Base::Vector3d& mov)
{
Base::Matrix4D mat;
mat.move(mov);
setTransform(mat * getTransform());
}
