C++ Affine3类代码示例

string MeshShapeItemImpl::toURDF()
{
    ostringstream ss;
    ss << "<link name=\"" << self->name() << "\">" << endl;
    Affine3 relative;
    JointItem* parentjoint = dynamic_cast<JointItem*>(self->parentItem());
    if (parentjoint) {
        Affine3 parent, child;
        parent.translation() = parentjoint->translation;
        parent.linear() = parentjoint->rotation;
        child.translation() = self->translation;
        child.linear() = self->rotation;
        relative = parent.inverse() * child;
    } else {
        relative.translation() = self->translation;
        relative.linear() = self->rotation;
    }
    for (int i=0; i < 2; i++) {
        if (i == 0) {
            ss << " <visual>" << endl;
        } else {
            ss << " <collision>" << endl;
        }
        if (i == 0) {
            ss << " </visual>" << endl;
        } else {
            ss << " </collision>" << endl;
        }
    }
    return ss.str();
}

double EuclidDistHeuristic::computeDistance(
    const Affine3& a,
    const Affine3& b) const
{
    auto sqrd = [](double d) { return d * d; };

    Vector3 diff = b.translation() - a.translation();

    double dp2 =
            m_x_coeff * sqrd(diff.x()) +
            m_y_coeff * sqrd(diff.y()) +
            m_z_coeff * sqrd(diff.z());

    Quaternion qb(b.rotation());
    Quaternion qa(a.rotation());

    double dot = qa.dot(qb);
    if (dot < 0.0) {
        qb = Quaternion(-qb.w(), -qb.x(), -qb.y(), -qb.z());
        dot = qa.dot(qb);
    }

    double dr2 = angles::normalize_angle(2.0 * std::acos(dot));
    dr2 *= (m_rot_coeff * dr2);

    SMPL_DEBUG_NAMED(LOG, "Compute Distance: sqrt(%f + %f)", dp2, dr2);

    return std::sqrt(dp2 + dr2);
}

Affine3 SgCamera::positionLookingFor(const Vector3& eye, const Vector3& direction, const Vector3& up)
{
    Vector3 d = direction.normalized();
    Vector3 c = d.cross(up).normalized();
    Vector3 u = c.cross(d);
    Affine3 C;
    C.linear() << c, u, -d;
    C.translation() = eye;
    return C;
}

bool RotationDragger::onButtonPressEvent(const SceneWidgetEvent& event)
{
    int axis;
    int indexOfTopNode;
    if(detectAxisFromNodePath(event.nodePath(), this, axis, indexOfTopNode)){
        Affine3 T = calcTotalTransform(event.nodePath(), this);
        dragProjector.setInitialPosition(T);
        dragProjector.setRotationAxis(T.linear().col(axis));
        if(dragProjector.startRotation(event)){
            sigRotationStarted_();
            return true;
        }
    }
    return false;
}

static inline void writeAffine3(YAMLWriter& writer, const Affine3& value)
{
    writer.startFlowStyleListing();

    writer.putScalar(value.translation().x());
    writer.putScalar(value.translation().y());
    writer.putScalar(value.translation().z());

    Vector3 rpy(rpyFromRot(value.linear()));
    writer.putScalar(rpy[0]);
    writer.putScalar(rpy[1]);
    writer.putScalar(rpy[2]);

    writer.endListing();
}

void VRMLBodyLoaderImpl::readJointSubNodes(LinkInfo& iLink, MFNode& childNodes, const ProtoIdSet& acceptableProtoIds, const Affine3& T)
{
    for(size_t i = 0; i < childNodes.size(); ++i){
        bool doTraverse = false;
        VRMLNode* childNode = childNodes[i].get();
        if(!childNode->isCategoryOf(PROTO_INSTANCE_NODE)){
            doTraverse = true;
        } else {
            VRMLProtoInstance* protoInstance = static_cast<VRMLProtoInstance*>(childNode);
            int id = PROTO_UNDEFINED;
            const string& protoName = protoInstance->proto->protoName;
            ProtoInfoMap::iterator p = protoInfoMap.find(protoName);
            if(p == protoInfoMap.end()){
                doTraverse = true;
                childNode = protoInstance->actualNode.get();
            } else {
                id = p->second.id;
                if(!acceptableProtoIds.test(id)){
                    throw invalid_argument(str(format(_("%1% node is not in a correct place.")) % protoName));
                }
                if(isVerbose){
                    messageIndent += 2;
                }
                switch(id){
                case PROTO_JOINT:
                    if(!T.matrix().isApprox(Affine3::MatrixType::Identity())){
                        throw invalid_argument(
                            str(format(_("Joint node \"%1%\" is not in a correct place.")) % protoInstance->defName));
                    }
                    iLink.link->appendChild(readJointNode(protoInstance, iLink.link->Rs()));
                    break;
                case PROTO_SEGMENT:
                    readSegmentNode(iLink, protoInstance, T);
                    linkOriginalMap[iLink.link] = childNodes[i];
                    break;
                case PROTO_SURFACE:
                    readSurfaceNode(iLink, protoInstance, T);
                    break;
                case PROTO_DEVICE:
                    readDeviceNode(iLink, protoInstance, T);
                    break;
                default:
                    doTraverse = true;
                    break;
                }
                if(isVerbose){
                    messageIndent -= 2;
                }
            }
        }
        if(doTraverse && childNode->isCategoryOf(GROUPING_NODE)){
            VRMLGroup* group = static_cast<VRMLGroup*>(childNode);
            if(VRMLTransform* transform = dynamic_cast<VRMLTransform*>(group)){
                readJointSubNodes(iLink, group->getChildren(), acceptableProtoIds, T * transform->toAffine3d());
            } else {
                readJointSubNodes(iLink, group->getChildren(), acceptableProtoIds, T);
            }
        }
    }
}

static void readAffine3(const Listing& node, Affine3& out_value)
{
    if(node.size() == 6){

        Affine3::TranslationPart t = out_value.translation();
        t[0] = node[0].toDouble();
        t[1] = node[1].toDouble();
        t[2] = node[2].toDouble();

        const double r = node[3].toDouble();
        const double p = node[4].toDouble();
        const double y = node[5].toDouble();
        
        out_value.linear() = rotFromRpy(r, p, y);
    }
}

void VRMLBodyLoaderImpl::readSurfaceNode(LinkInfo& iLink, VRMLProtoInstance* segmentShapeNode, const Affine3& T)
{
    const string& typeName = segmentShapeNode->proto->protoName;
    if(isVerbose) putMessage(string("Surface node ") + segmentShapeNode->defName);
    
    iLink.isSurfaceNodeUsed = true;

    // check if another Surface node does not appear in the subtree
    MFNode& visualNodes = get<MFNode>(segmentShapeNode->fields["visual"]);
    ProtoIdSet acceptableProtoIds;
    readJointSubNodes(iLink, visualNodes, acceptableProtoIds, T);
    MFNode& collisionNodes = get<MFNode>(segmentShapeNode->fields["collision"]);
    readJointSubNodes(iLink, collisionNodes, acceptableProtoIds, T);

    SgGroup* group;
    SgPosTransform* transform = 0;
    if(T.isApprox(Affine3::Identity())){
        group = iLink.collisionShape;
    } else {
        transform = new SgPosTransform(T);
        group = transform;
    }
    for(size_t i=0; i < collisionNodes.size(); ++i){
        SgNodePtr node = sgConverter.convert(collisionNodes[i]);
        if(node){
            group->addChild(node);
        }
    }
    if(transform && !transform->empty()){
        iLink.collisionShape->addChild(transform);
    }
}

bool PhongShadowProgram::renderLight(int index, const SgLight* light, const Affine3& T, const Affine3& viewMatrix, bool shadowCasting)
{
    LightInfo& info = lightInfos[index];

    if(const SgDirectionalLight* dirLight = dynamic_cast<const SgDirectionalLight*>(light)){
        Vector3 d = viewMatrix.linear() * T.linear() * -dirLight->direction();
        Vector4f pos(d.x(), d.y(), d.z(), 0.0f);
        glUniform4fv(info.positionLocation, 1, pos.data());

    } else if(const SgPointLight* pointLight = dynamic_cast<const SgPointLight*>(light)){
        Vector3 p(viewMatrix * T.translation());
        Vector4f pos(p.x(), p.y(), p.z(), 1.0f);
        glUniform4fv(info.positionLocation, 1, pos.data());
        glUniform1f(info.constantAttenuationLocation, pointLight->constantAttenuation());
        glUniform1f(info.linearAttenuationLocation, pointLight->linearAttenuation());
        glUniform1f(info.quadraticAttenuationLocation, pointLight->quadraticAttenuation());
        
        if(const SgSpotLight* spotLight = dynamic_cast<const SgSpotLight*>(pointLight)){
            Vector3 d = viewMatrix.linear() * T.linear() * spotLight->direction();
            Vector3f direction(d.cast<float>());
            glUniform3fv(info.directionLocation, 1, direction.data());
            glUniform1f(info.cutoffAngleLocation, spotLight->cutOffAngle());
            glUniform1f(info.beamWidthLocation, spotLight->beamWidth());
            glUniform1f(info.cutoffExponentLocation, spotLight->cutOffExponent());
        }
    } else {
        return false;
    }
        
    Vector3f intensity(light->intensity() * light->color());
    glUniform3fv(info.intensityLocation, 1, intensity.data());
    Vector3f ambientIntensity(light->ambientIntensity() * light->color());
    glUniform3fv(info.ambientIntensityLocation, 1, ambientIntensity.data());

    if(shadowCasting){
        if(currentShadowIndex < numShadows_){
            ShadowInfo& shadow = shadowInfos[currentShadowIndex];
            glUniform1i(shadow.shadowMapLocation, currentShadowIndex + 1);
            glUniform1i(shadow.lightIndexLocation, index);
            ++currentShadowIndex;
        }
    }

    return true;
}

void VRMLBodyLoaderImpl::readSegmentNode(LinkInfo& iLink, VRMLProtoInstance* segmentNode, const Affine3& T)
{
    if(isVerbose) putMessage(string("Segment node ") + segmentNode->defName);
    
    /*
      Mass = Sigma mass 
      C = (Sigma mass * T * c) / Mass 
      I = Sigma(R * I * Rt + G)       
      R = Rotation matrix part of T   
      G = y*y+z*z, -x*y, -x*z, -y*x, z*z+x*x, -y*z, -z*x, -z*y, x*x+y*y    
      (x, y, z ) = T * c - C
    */

    VRMLProtoFieldMap& sf = segmentNode->fields;
    SegmentInfo iSegment;
    readVRMLfield(sf["mass"], iSegment.m);
    Vector3 c;
    readVRMLfield(sf["centerOfMass"], c);
    iSegment.c = T.linear() * c + T.translation();
    iLink.c = (iSegment.c * iSegment.m + iLink.c * iLink.m) / (iLink.m + iSegment.m);
    iLink.m += iSegment.m;
    
    Matrix3 I;
    readVRMLfield(sf["momentsOfInertia"], I);
    iLink.I.noalias() += T.linear() * I * T.linear().transpose();

    MFNode& childNodes = get<MFNode>(segmentNode->fields["children"]);
    ProtoIdSet acceptableProtoIds;
    acceptableProtoIds.set(PROTO_SURFACE);
    acceptableProtoIds.set(PROTO_DEVICE);
    readJointSubNodes(iLink, childNodes, acceptableProtoIds, T);

    SgNodePtr node = sgConverter.convert(segmentNode);
    if(node){
        if(T.isApprox(Affine3::Identity())){
            iLink.visualShape->addChild(node);
        } else {
            SgPosTransform* transform = new SgPosTransform(T);
            transform->addChild(node);
            iLink.visualShape->addChild(transform);
        }
    }
}

void SfmData::applyTransform(const Affine3<FltType> &M) {
	auto M_inv = M.inv();
	for (auto& cam : cameras) {
		auto tmp = cam.pose()*M_inv;
		cam.R = tmp.rotation();
		cam.t = tmp.translation();
	}
	for (auto &p : cloud) {
		p = M*p;
	}
}

int EuclidDistHeuristic::GetGoalHeuristic(int state_id)
{
    if (state_id == planningSpace()->getGoalStateID()) {
        return 0;
    }

    if (m_pose_ext) {
        Affine3 p;
        if (!m_pose_ext->projectToPose(state_id, p)) {
            return 0;
        }

        auto& goal_pose = planningSpace()->goal().pose;

        const double dist = computeDistance(p, goal_pose);

        const int h = FIXED_POINT_RATIO * dist;

        double Y, P, R;
        angles::get_euler_zyx(p.rotation(), Y, P, R);
        SMPL_DEBUG_NAMED(LOG, "h(%0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f) = %d", p.translation()[0], p.translation()[1], p.translation()[2], Y, P, R, h);

        return h;
    } else if (m_point_ext) {
        Vector3 p;
        if (!m_point_ext->projectToPoint(state_id, p)) {
            return 0;
        }

        auto& goal_pose = planningSpace()->goal().pose;
        Vector3 gp(goal_pose.translation());

        double dist = computeDistance(p, gp);

        const int h = FIXED_POINT_RATIO * dist;
        SMPL_DEBUG_NAMED(LOG, "h(%d) = %d", state_id, h);
        return h;
    } else {
        return 0;
    }
}

void PhongShadowProgram::setTransformMatrices(const Affine3& viewMatrix, const Affine3& modelMatrix, const Matrix4& PV)
{
    const Affine3f VM = (viewMatrix * modelMatrix).cast<float>();
    const Matrix3f N = VM.linear();
    const Matrix4f PVM = (PV * modelMatrix.matrix()).cast<float>();

    if(useUniformBlockToPassTransformationMatrices){
       transformBlockBuffer.write(modelViewMatrixIndex, VM);
       transformBlockBuffer.write(normalMatrixIndex, N);
       transformBlockBuffer.write(MVPIndex, PVM);
       transformBlockBuffer.flush();
    } else {
        glUniformMatrix4fv(modelViewMatrixLocation, 1, GL_FALSE, VM.data());
        glUniformMatrix3fv(normalMatrixLocation, 1, GL_FALSE, N.data());
        glUniformMatrix4fv(MVPLocation, 1, GL_FALSE, PVM.data());
        for(int i=0; i < numShadows_; ++i){
            ShadowInfo& shadow = shadowInfos[i];
            const Matrix4f BPVM = (shadow.BPV * modelMatrix.matrix()).cast<float>();
            glUniformMatrix4fv(shadow.shadowMatrixLocation, 1, GL_FALSE, BPVM.data());
        }
    }
}

void cnoid::savePCD(SgPointSet* pointSet, const std::string& filename, const Affine3& viewpoint)
{
    if(!pointSet->hasVertices()){
        throw empty_data_error() << error_info_message(_("Empty pointset"));
    }

    ofstream ofs;
    ofs.open(filename.c_str());

    ofs <<
        "# .PCD v.7 - Point Cloud Data file format\n"
        "VERSION .7\n"
        "FIELDS x y z\n"
        "SIZE 4 4 4\n"
        "TYPE F F F\n"
        "COUNT 1 1 1\n";

    const SgVertexArray& points = *pointSet->vertices();
    const int numPoints = points.size();
    ofs << "WIDTH " << numPoints << "\n";
    ofs << "HEIGHT 1\n";

    ofs << "VIEWPOINT ";
    Affine3::ConstTranslationPart t = viewpoint.translation();
    ofs << t.x() << " " << t.y() << " " << t.z() << " ";
    const Quaternion q(viewpoint.rotation());
    ofs << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << "\n";

    ofs << "POINTS " << numPoints << "\n";
    
    ofs << "DATA ascii\n";

    for(int i=0; i < numPoints; ++i){
        const Vector3f& p = points[i];
        ofs << p.x() << " " << p.y() << " " << p.z() << "\n";
    }

    ofs.close();
}

void VRMLBodyLoaderImpl::readDeviceNode(LinkInfo& iLink, VRMLProtoInstance* deviceNode, const Affine3& T)
{
    const string& typeName = deviceNode->proto->protoName;
    if(isVerbose) putMessage(typeName + " node " + deviceNode->defName);
    
    DeviceFactoryMap::iterator p = deviceFactories.find(typeName);
    if(p == deviceFactories.end()){
        os() << str(format("Sensor type %1% is not supported.\n") % typeName) << endl;
    } else {
        DeviceFactory& factory = p->second;
        DevicePtr device = factory(deviceNode);
        if(device){
            device->setLink(iLink.link);
            const Matrix3 RsT = iLink.link->Rs();
            device->setLocalTranslation(RsT * (T * device->localTranslation()));
            device->setLocalRotation(RsT * (T.linear() * device->localRotation()));
            body->addDevice(device);
        }
    }
}