C++ base::Matrix4D类代码示例

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();
}

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