Exemple #1
0
wb_adrep *wb_bdrep::adrep( pwr_tStatus *sts, const char *aname)
{
#if 0
  wb_attrname n(aname);
  if ( n.evenSts()) {
    *sts = n.sts();
    return 0;
  }

  char fullname[120];
  wb_bdrep *bd = this;
  wb_adrep *adrep = 0;
  int offset;

  for ( int i = 0; i < n.attributes(); i++) {
    wb_name an(n.attribute(i));
    wb_orep *orep = bd->m_orep->vrep()->child( sts, bd->m_orep, an);
    if ( EVEN(*sts)) return 0;

    if ( adrep)
      delete adrep;
    adrep = new wb_adrep( *orep);
    if ( i == 0) {
      offset = adrep->offset();
      strcpy( fullname, adrep->name());
    }
    else {
      offset += adrep->offset();
      strcat( fullname, ".");
      strcat( fullname, adrep->name());
    }
    if ( n.hasAttrIndex(i)) {
      sprintf( &fullname[strlen(fullname)], "[%d]", n.attrIndex(i));
      if ( n.attrIndex(i) >= adrep->nElement() || n.attrIndex(i) < 0) {
	*sts = LDH__ATTRINDEX;
	return 0;
      }

      offset += n.attrIndex(i) * adrep->size() / adrep->nElement();
    }

    if ( (i != n.attributes() - 1) && adrep->isClass()) {
      wb_cdrep *cd = m_orep->vrep()->merep()->cdrep( sts, adrep->subClass());
      if ( EVEN(*sts)) return 0;

      if ( bd != this)
	delete bd;
      bd = cd->bdrep( sts, pwr_eBix_rt);
      if ( EVEN(*sts)) { delete cd; return 0;}

      delete cd;
    }

  }
  adrep->setSubattr( offset, fullname);
#endif

  wb_attrname n(aname);
  if ( n.evenSts()) {
    *sts = n.sts();
    return 0;
  }

  wb_bdrep *bd = this;
  wb_adrep *adrep = 0;
  wb_adrep *old = 0;

  for ( int i = 0; i < n.attributes(); i++) {
    bool next_attr = false;
    wb_name an(n.attribute(i));
    wb_orep *orep = bd->m_orep->vrep()->child( sts, bd->m_orep, an);
    while ( EVEN(*sts)) {      
      // Try Super attribute
      orep = bd->m_orep->vrep()->first( sts, bd->m_orep);
      if ( EVEN(*sts)) {
	if ( bd != this) delete bd;
	if ( adrep) delete adrep;
	return 0;
      }
	
      if ( cdh_NoCaseStrcmp( orep->name(), "Super") == 0) {
	if ( adrep)
	  old = adrep;

	adrep = new wb_adrep( *orep);
	if ( old)
	  adrep->add( old);
	delete old;

	wb_cdrep *cd = m_orep->vrep()->merep()->cdrep( sts, adrep->subClass());
	if ( EVEN(*sts)) return 0;

	if ( bd != this)
	  delete bd;
	bd = cd->bdrep( sts, pwr_eBix_rt);
	if ( EVEN(*sts)) { delete cd; return 0;}

	delete cd;

	orep = bd->m_orep->vrep()->child( sts, bd->m_orep, an);
      }
      else {
	if ( adrep && adrep->flags() & PWR_MASK_CASTATTR && n.hasSuper()) {
	  // Allow additional super attributesegement for casted attributes
	  next_attr = true;
	  break;
	}
	orep->ref();
	orep->unref();
	*sts = LDH__NOSUCHATTR;
	return 0;
      }
    }
    if ( next_attr)
      continue;
    if ( adrep)
      old = adrep;

    adrep = new wb_adrep( *orep);
    if ( i != 0) {
      if ( n.hasAttrIndex( i - 1))
	adrep->add( old, n.attrIndex( i - 1));
      else
	adrep->add( old);
      delete old;
    }
    if ( n.hasAttrIndex(i) &&
	 (n.attrIndex(i) >= adrep->nElement() || n.attrIndex(i) < 0)) {
      *sts = LDH__ATTRINDEX;
      return 0;
    }

    if ( (i != n.attributes() - 1) && adrep->isClass()) {
      wb_cdrep *cd;
      if ( m_merep)
	cd = m_merep->cdrep( sts, adrep->subClass());
      else
	cd = m_orep->vrep()->merep()->cdrep( sts, adrep->subClass());
      if ( EVEN(*sts)) return 0;

      if ( bd != this)
	delete bd;
      bd = cd->bdrep( sts, pwr_eBix_rt);
      if ( EVEN(*sts)) { delete cd; return 0;}

      delete cd;
    }
    else if ( (i != n.attributes() - 1) && !adrep->isClass()) {
      // To many attribute
      delete adrep;
      *sts = LDH__NOSUCHATTR;
      return 0;
    }

  }

  return adrep;
}
Exemple #2
0
	void Model::create(const std::string& filepath)
	{
		std::ifstream fin(filepath.c_str(), std::ios::in | std::ios::binary);

		if (fin.is_open())
		{
			ResHeader header = {};
			fin.read(reinterpret_cast<char*>(&header), sizeof(header));

			m_bindingDescriptions[0].binding = 0;
			m_bindingDescriptions[0].stride = sizeof(Vertex);
			m_bindingDescriptions[0].inputRate = VK_VERTEX_INPUT_RATE_VERTEX;

			m_attributeDescriptions[0].binding = 0;
			m_attributeDescriptions[0].location = 0;
			m_attributeDescriptions[0].format = VK_FORMAT_R32G32B32_SFLOAT;
			m_attributeDescriptions[0].offset = 0;

			m_attributeDescriptions[1].binding = 0;
			m_attributeDescriptions[1].location = 1;
			m_attributeDescriptions[1].format = VK_FORMAT_R32G32B32_SFLOAT;
			m_attributeDescriptions[1].offset = sizeof(float) * 3;

			m_vertexInputInfo.sType = VK_STRUCTURE_TYPE_PIPELINE_VERTEX_INPUT_STATE_CREATE_INFO;
			m_vertexInputInfo.pNext = NULL;
			m_vertexInputInfo.flags = 0;
			m_vertexInputInfo.vertexBindingDescriptionCount = 1;
			m_vertexInputInfo.pVertexBindingDescriptions = &m_bindingDescriptions[0];
			m_vertexInputInfo.vertexAttributeDescriptionCount = 2;
			m_vertexInputInfo.pVertexAttributeDescriptions = &m_attributeDescriptions[0];

			std::unique_ptr<ResVec3> vertices;
			std::unique_ptr<ResFace> faces;
			
			vertices.reset(new ResVec3[header.numVerts]);
			faces.reset(new ResFace[header.numFaces]);

			fin.read(reinterpret_cast<char*>(vertices.get()), sizeof(ResVec3) * header.numVerts);
			fin.read(reinterpret_cast<char*>(faces.get()), sizeof(ResFace) * header.numFaces);

			{
				VkBufferCreateInfo bufInfo = {};
				bufInfo.sType = VK_STRUCTURE_TYPE_BUFFER_CREATE_INFO;
				bufInfo.pNext = NULL;
				bufInfo.size = getDeviceSize(sizeof(Vertex) * header.numVerts, 256);
				bufInfo.usage = VK_BUFFER_USAGE_VERTEX_BUFFER_BIT;
				bufInfo.flags = 0;

				auto err = vkCreateBuffer(getDevice(), &bufInfo, nullptr, &m_vertices.buffer);

				VkMemoryRequirements memReqs = {};
				vkGetBufferMemoryRequirements(getDevice(), m_vertices.buffer, &memReqs);

				VkMemoryAllocateInfo memAlloc = {};
				memAlloc.sType = VK_STRUCTURE_TYPE_MEMORY_ALLOCATE_INFO;
				memAlloc.allocationSize = memReqs.size;
				getMemoryType(memReqs.memoryTypeBits, VK_MEMORY_PROPERTY_HOST_VISIBLE_BIT, &memAlloc.memoryTypeIndex);

				err = vkAllocateMemory(getDevice(), &memAlloc, nullptr, &m_vertices.memory);


				Vertex* mapped = nullptr;
				err = vkMapMemory(getDevice(), m_vertices.memory, 0, bufInfo.size, 0, reinterpret_cast<void**>(&mapped));

				for (uint32_t i = 0; i < header.numVerts; i++)
				{
					mapped[i].x = vertices.get()[i].x;
					mapped[i].y = vertices.get()[i].y;
					mapped[i].z = vertices.get()[i].z;

					mapped[i].nx = 0.0f;
					mapped[i].ny = 0.0f;
					mapped[i].nz = 0.0f;
				}

				for (uint32_t i = 0; i < header.numFaces; i++)
				{
					Vertex* p0 = &mapped[faces.get()[i].index[0]];
					Vertex* p1 = &mapped[faces.get()[i].index[1]];
					Vertex* p2 = &mapped[faces.get()[i].index[2]];

					p0->nx += faces.get()[i].normal.x;
					p0->ny += faces.get()[i].normal.y;
					p0->nz += faces.get()[i].normal.z;

					p1->nx += faces.get()[i].normal.x;
					p1->ny += faces.get()[i].normal.y;
					p1->nz += faces.get()[i].normal.z;

					p2->nx += faces.get()[i].normal.x;
					p2->ny += faces.get()[i].normal.y;
					p2->nz += faces.get()[i].normal.z;
				}

				for (uint32_t i = 0; i < header.numVerts; i++)
				{
					glm::vec3 n(mapped[i].nx, mapped[i].ny, mapped[i].nz);
					n = glm::normalize(n);
					mapped[i].nx = n.x;
					mapped[i].ny = n.y;
					mapped[i].nz = n.z;
				}

				vkUnmapMemory(getDevice(), m_vertices.memory);
				vkBindBufferMemory(getDevice(), m_vertices.buffer, m_vertices.memory, 0);
			}

			{
				VkBufferCreateInfo bufInfo = {};
				bufInfo.sType = VK_STRUCTURE_TYPE_BUFFER_CREATE_INFO;
				bufInfo.pNext = NULL;
				bufInfo.size = getDeviceSize(sizeof(uint32_t) * header.numFaces * 3, 256);
				bufInfo.usage = VK_BUFFER_USAGE_INDEX_BUFFER_BIT;
				bufInfo.flags = 0;

				auto err = vkCreateBuffer(getDevice(), &bufInfo, nullptr, &m_indices.buffer);

				VkMemoryRequirements memReqs = {};
				vkGetBufferMemoryRequirements(getDevice(), m_indices.buffer, &memReqs);

				VkMemoryAllocateInfo memAlloc = {};
				memAlloc.sType = VK_STRUCTURE_TYPE_MEMORY_ALLOCATE_INFO;
				memAlloc.allocationSize = memReqs.size;
				getMemoryType(memReqs.memoryTypeBits, VK_MEMORY_PROPERTY_HOST_VISIBLE_BIT, &memAlloc.memoryTypeIndex);

				err = vkAllocateMemory(getDevice(), &memAlloc, nullptr, &m_indices.memory);

				uint32_t* mapped = nullptr;
				err = vkMapMemory(getDevice(), m_indices.memory, 0, bufInfo.size, 0, reinterpret_cast<void**>(&mapped));

				uint32_t index = 0;
				for (uint32_t i = 0; i < header.numFaces; i++)
				{
					mapped[index + 0] = faces.get()[i].index[0];
					mapped[index + 1] = faces.get()[i].index[1];
					mapped[index + 2] = faces.get()[i].index[2];

					index += 3;
				}

				vkUnmapMemory(getDevice(), m_indices.memory);
				vkBindBufferMemory(getDevice(), m_indices.buffer, m_indices.memory, 0);

				m_indexCount = header.numFaces * 3;
			}

			fin.close();
		}
	}
void MeshAlg::computeTangentSpaceBasis(
    const Array<Vector3>&       vertexArray,
    const Array<Vector2>&       texCoordArray,
    const Array<Vector3>&       vertexNormalArray,
    const Array<Face>&          faceArray,
    Array<Vector3>&             tangent,
    Array<Vector3>&             binormal) {

    debugAssertM(faceArray.size() != 0, "Unable to calculate valid tangent space without faces.");

    tangent.resize(vertexArray.size());
    binormal.resize(vertexArray.size());

    // Zero the output arrays.
    System::memset(tangent.getCArray(), 0, sizeof(Vector3) * tangent.size());
    System::memset(binormal.getCArray(), 0, sizeof(Vector3) * binormal.size());

    // Iterate over faces, computing the tangent vectors for each
    // vertex.  Accumulate those into the tangent and binormal arrays
    // and then orthonormalize at the end.

    for (int f = 0; f < faceArray.size(); ++f) {
        const Face& face = faceArray[f];

        const int i0 = face.vertexIndex[0];
        const int i1 = face.vertexIndex[1];
        const int i2 = face.vertexIndex[2];

        const Vector3& v0 = vertexArray[i0];
        const Vector3& v1 = vertexArray[i1];
        const Vector3& v2 = vertexArray[i2];

        const Vector2& t0 = texCoordArray[i0];
        const Vector2& t1 = texCoordArray[i1];
        const Vector2& t2 = texCoordArray[i2];

        // See http://www.terathon.com/code/tangent.html for a derivation of the following code

        // vertex edges
        Vector3 ve1 = v1 - v0;
        Vector3 ve2 = v2 - v0;

        // texture edges
        Vector2 te1 = t1 - t0;
        Vector2 te2 = t2 - t0;

        Vector3 n(ve1.cross(ve2).direction());
        Vector3 t, b;

        float r = te1.x * te2.y - te1.y * te2.x;
        if (r == 0.0) {
            // degenerate case
            Vector3::generateOrthonormalBasis(t, b, n, true);
        } else {
            r = 1.0f / r;
            t = (te2.y * ve1 - te1.y * ve2) * r;
            b = (te2.x * ve1 - te1.x * ve2) * r;
        }

        for (int v = 0; v < 3; ++v) {
            int i = face.vertexIndex[v];
            tangent[i]  += t;
            binormal[i] += b;
        }
    }

    // Normalize the basis vectors
    for (int v = 0; v < vertexArray.size(); ++v) {
        // Remove the component parallel to the normal
        const Vector3& N = vertexNormalArray[v];
        Vector3& T = tangent[v];
        Vector3& B = binormal[v];

        debugAssertM(N.isUnit() || N.isZero(), "Input normals must have unit length");

        T -= T.dot(N) * N;
        B -= B.dot(N) * N;

        // Normalize
        T = T.directionOrZero();
        B = B.directionOrZero();
    }
}
Exemple #4
0
void SurfaceTensionBoundaryCondition :: computeLoadVectorFromElement(FloatArray &answer, Element *e, int side, TimeStep *tStep)
{
    FEInterpolation *fei = e->giveInterpolation();
    if ( !fei ) {
        OOFEM_ERROR("No interpolation or default integration available for element.");
    }
    std :: unique_ptr< IntegrationRule > iRule( fei->giveBoundaryIntegrationRule(fei->giveInterpolationOrder()-1, side) );

    int nsd = e->giveDomain()->giveNumberOfSpatialDimensions();

    if ( side == -1 ) {
        side = 1;
    }

    answer.clear();

    if ( nsd == 2 ) {

        FEInterpolation2d *fei2d = static_cast< FEInterpolation2d * >(fei);

        ///@todo More of this grunt work should be moved to the interpolation classes
        IntArray bnodes;
        fei2d->boundaryGiveNodes(bnodes, side);
        int nodes = bnodes.giveSize();
        FloatMatrix xy(2, nodes);
        for ( int i = 1; i <= nodes; i++ ) {
            Node *node = e->giveNode(bnodes.at(i));
            ///@todo This should rely on the xindex and yindex in the interpolator..
            xy.at(1, i) = node->giveCoordinate(1);
            xy.at(2, i) = node->giveCoordinate(2);
        }

        FloatArray tmp; // Integrand
        FloatArray es; // Tangent vector to curve
        FloatArray dNds;

        if ( e->giveDomain()->isAxisymmetric() ) {
            FloatArray N;
            FloatArray gcoords;
            for ( GaussPoint *gp: *iRule ) {
                fei2d->edgeEvaldNds( dNds, side, * gp->giveCoordinates(), FEIElementGeometryWrapper(e) );
                fei->boundaryEvalN( N, side, * gp->giveCoordinates(), FEIElementGeometryWrapper(e) );
                double J = fei->boundaryGiveTransformationJacobian( side, * gp->giveCoordinates(), FEIElementGeometryWrapper(e) );
                fei->boundaryLocal2Global( gcoords, side, * gp->giveCoordinates(), FEIElementGeometryWrapper(e) );
                double r = gcoords(0); // First coordinate is the radial coord.

                es.beProductOf(xy, dNds);

                tmp.resize( 2 * nodes);
                for ( int i = 0; i < nodes; i++ ) {
                    tmp(2 * i)   = dNds(i) * es(0) * r + N(i);
                    tmp(2 * i + 1) = dNds(i) * es(1) * r;
                }

                answer.add(- 2 * M_PI * gamma * J * gp->giveWeight(), tmp);
            }
        } else {
            for ( GaussPoint *gp: *iRule ) {
                double t = e->giveCrossSection()->give(CS_Thickness, gp);
                fei2d->edgeEvaldNds( dNds, side, * gp->giveCoordinates(), FEIElementGeometryWrapper(e) );
                double J = fei->boundaryGiveTransformationJacobian( side, * gp->giveCoordinates(), FEIElementGeometryWrapper(e) );
                es.beProductOf(xy, dNds);

                tmp.resize( 2 * nodes);
                for ( int i = 0; i < nodes; i++ ) {
                    tmp(2 * i)   = dNds(i) * es(0);
                    tmp(2 * i + 1) = dNds(i) * es(1);
                    //B.at(1, 1+i*2) = B.at(2, 2+i*2) = dNds(i);
                }
                //tmp.beTProductOf(B, es);

                answer.add(- t * gamma * J * gp->giveWeight(), tmp);
            }
        }
    } else if ( nsd ==  3 ) {

        FEInterpolation3d *fei3d = static_cast< FEInterpolation3d * >(fei);
        FloatArray n, surfProj;
        FloatMatrix dNdx, B;
        for ( GaussPoint *gp: *iRule ) {
            fei3d->surfaceEvaldNdx( dNdx, side, * gp->giveCoordinates(), FEIElementGeometryWrapper(e) );
            double J = fei->boundaryEvalNormal( n, side, * gp->giveCoordinates(), FEIElementGeometryWrapper(e) );

            // [I - n(x)n]  in voigt form:
            surfProj = {1. - n(0)*n(0), 1. - n(1)*n(1), 1. - n(2)*n(2),
                        - n(1)*n(2), - n(0)*n(2), - n(0)*n(1),
            };

            // Construct B matrix of the surface nodes
            B.resize(6, 3 * dNdx.giveNumberOfRows());
            for ( int i = 1; i <= dNdx.giveNumberOfRows(); i++ ) {
                B.at(1, 3 * i - 2) = dNdx.at(i, 1);
                B.at(2, 3 * i - 1) = dNdx.at(i, 2);
                B.at(3, 3 * i - 0) = dNdx.at(i, 3);

                B.at(5, 3 * i - 2) = B.at(4, 3 * i - 1) = dNdx.at(i, 3);
                B.at(6, 3 * i - 2) = B.at(4, 3 * i - 0) = dNdx.at(i, 2);
                B.at(6, 3 * i - 1) = B.at(5, 3 * i - 0) = dNdx.at(i, 1);
            }

            answer.plusProduct(B, surfProj, -gamma * J * gp->giveWeight() );
        }
    }
}
bool ChainParser::parse(Tree& tree,
                        std::map<std::string,unsigned int>& joint_name_to_index,
                        std::vector<std::string>& index_to_joint_name,
                        KDL::JntArray& q_min,
                        KDL::JntArray& q_max)
{
    ros::NodeHandle n("~");

    /* * * * * * * * PARSE JOINT TOPICS * * * * * * * * */


    /* * * * * * * * PARSE URDF * * * * * * * * */

    std::string urdf_xml, full_urdf_xml;
    n.param("urdf_xml", urdf_xml,std::string("/amigo/robot_description"));
    n.searchParam(urdf_xml, full_urdf_xml);
    ROS_INFO("Reading xml file from parameter server");
    std::string result_string;
    if (!n.getParam(full_urdf_xml, result_string)) {
        ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
        //error = true;
        return false;
    }

    urdf::Model robot_model;

    // Is this necessary?
    if (!robot_model.initString(result_string)) {
        ROS_FATAL("Could not initialize robot model");
        //error = true;
        return -1;
    }
    ROS_INFO("Robot model initialized");

    if (!kdl_parser::treeFromString(result_string, tree.kdl_tree_)) {
        ROS_ERROR("Could not initialize tree object");
        //error = true;
        return false;
    }
    ROS_INFO("Tree object initialized");

    /* * * * * * * * PARSE CHAINS * * * * * * * * */

    XmlRpc::XmlRpcValue chain_params;
    if (!n.getParam("chain_description", chain_params)) {
        ROS_ERROR("No chain description given: %s", "chain_description");
        return false;
    }

    if (chain_params.getType() != XmlRpc::XmlRpcValue::TypeArray) {
        ROS_ERROR("Chain description list should be an array.  (namespace: %s)", n.getNamespace().c_str());
        return false;
    }

    std::vector<double> q_min_vec;
    std::vector<double> q_max_vec;

    for(int i = 0; i < chain_params.size(); ++i) {
        ROS_INFO("Going to parse chain ...");
        //Chain* chain = parseChain(chain_params[i], tree, robot_model, joint_name_to_index, index_to_joint_name, q_min_vec, q_max_vec);
        parseChain(chain_params[i], tree, robot_model, joint_name_to_index, index_to_joint_name, q_min_vec, q_max_vec);
        ROS_INFO("Parse chain ready");
        //if (chain) {
        //    chains.push_back(chain);
        //}
    }

    q_min.resize(joint_name_to_index.size());
    q_max.resize(joint_name_to_index.size());

    for(unsigned int i = 0; i < joint_name_to_index.size(); ++i) {
        q_min(i) = q_min_vec[i];
        q_max(i) = q_max_vec[i];
    }

    ROS_INFO("Parsing done");

    return true;
}
Exemple #6
0
 void NamespaceIndex::add_ns( OperationContext* txn,
                              const StringData& ns, const NamespaceDetails* details ) {
     Namespace n(ns);
     add_ns( txn, n, details );
 }
dgInt32 dgPolygonSoupDatabaseBuilder::FilterFace(dgInt32 count,
    dgInt32* const pool)
{
  if (count == 3)
  {
    dgBigVector p0(m_vertexPoints[pool[2]]);
    for (dgInt32 i = 0; i < 3; i++)
    {
      dgBigVector p1(m_vertexPoints[pool[i]]);
      dgBigVector edge(p1 - p0);
      dgFloat64 mag2 = edge % edge;
      if (mag2 < dgFloat32(1.0e-6f))
      {
        count = 0;
      }
      p0 = p1;
    }

    if (count == 3)
    {
      dgBigVector edge0(m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
      dgBigVector edge1(m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
      dgBigVector normal(edge0 * edge1);
      dgFloat64 mag2 = normal % normal;
      if (mag2 < dgFloat32(1.0e-8f))
      {
        count = 0;
      }
    }
  }
  else
  {
    dgPolySoupFilterAllocator polyhedra(m_allocator);

    count = polyhedra.AddFilterFace(dgUnsigned32(count), pool);

    if (!count)
    {
      return 0;
    }

    dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
    if (edge->m_incidentFace < 0)
    {
      edge = edge->m_twin;
    }

    bool flag = true;
    while (flag)
    {
      flag = false;
      if (count >= 3)
      {
        dgEdge* ptr = edge;

        dgBigVector p0(&m_vertexPoints[ptr->m_incidentVertex].m_x);
        do
        {
          dgBigVector p1(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
          dgBigVector e0(p1 - p0);
          dgFloat64 mag2 = e0 % e0;
          if (mag2 < dgFloat32(1.0e-6f))
          {
            count--;
            flag = true;
            edge = ptr->m_next;
            ptr->m_prev->m_next = ptr->m_next;
            ptr->m_next->m_prev = ptr->m_prev;
            ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
            ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
            break;
          }
          p0 = p1;
          ptr = ptr->m_next;
        } while (ptr != edge);
      }
    }
    if (count >= 3)
    {
      flag = true;
      dgBigVector normal(
          polyhedra.FaceNormal(edge, &m_vertexPoints[0].m_x,
              sizeof(dgBigVector)));

      _ASSERTE((normal % normal) > dgFloat32 (1.0e-10f));
      normal = normal.Scale(dgRsqrt (normal % normal + dgFloat32 (1.0e-20f)));

      while (flag)
      {
        flag = false;
        if (count >= 3)
        {
          dgEdge* ptr = edge;

          dgBigVector p0(&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
          dgBigVector p1(&m_vertexPoints[ptr->m_incidentVertex].m_x);
          dgBigVector e0(p1 - p0);
          e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
          do
          {
            dgBigVector p2(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
            dgBigVector e1(p2 - p1);

            e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
            dgFloat64 mag2 = e1 % e0;
            if (mag2 > dgFloat32(0.9999f))
            {
              count--;
              flag = true;
              edge = ptr->m_next;
              ptr->m_prev->m_next = ptr->m_next;
              ptr->m_next->m_prev = ptr->m_prev;
              ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
              ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
              break;
            }

            dgBigVector n(e0 * e1);
            mag2 = n % normal;
            if (mag2 < dgFloat32(1.0e-5f))
            {
              count--;
              flag = true;
              edge = ptr->m_next;
              ptr->m_prev->m_next = ptr->m_next;
              ptr->m_next->m_prev = ptr->m_prev;
              ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
              ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
              break;
            }

            e0 = e1;
            p1 = p2;
            ptr = ptr->m_next;
          } while (ptr != edge);
        }
      }
    }

    dgEdge* first = edge;
    if (count >= 3)
    {
      dgFloat64 best = dgFloat32(2.0f);
      dgEdge* ptr = edge;

      dgBigVector p0(&m_vertexPoints[ptr->m_incidentVertex].m_x);
      dgBigVector p1(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
      dgBigVector e0(p1 - p0);
      e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
      do
      {
        dgBigVector p2(
            &m_vertexPoints[ptr->m_next->m_next->m_incidentVertex].m_x);
        dgBigVector e1(p2 - p1);

        e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
        dgFloat64 mag2 = fabs(e1 % e0);
        if (mag2 < best)
        {
          best = mag2;
          first = ptr;
        }

        e0 = e1;
        p1 = p2;
        ptr = ptr->m_next;
      } while (ptr != edge);

      count = 0;
      ptr = first;
      do
      {
        pool[count] = ptr->m_incidentVertex;
        count++;
        ptr = ptr->m_next;
      } while (ptr != first);
    }

#ifdef _DEBUG
    if (count >= 3)
    {
      dgInt32 j0 = count - 2;
      dgInt32 j1 = count - 1;
      dgBigVector normal(
          polyhedra.FaceNormal(edge, &m_vertexPoints[0].m_x,
              sizeof(dgBigVector)));
      for (dgInt32 j2 = 0; j2 < count; j2++)
      {
        dgBigVector p0(&m_vertexPoints[pool[j0]].m_x);
        dgBigVector p1(&m_vertexPoints[pool[j1]].m_x);
        dgBigVector p2(&m_vertexPoints[pool[j2]].m_x);
        dgBigVector e0((p0 - p1));
        dgBigVector e1((p2 - p1));

        dgBigVector n(e1 * e0);
        _ASSERTE((n % normal) > dgFloat32 (0.0f));
        j0 = j1;
        j1 = j2;
      }
    }
#endif
  }

  return (count >= 3) ? count : 0;
}
Exemple #8
0
// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
void b2CollideEdgeAndCircle(b2Manifold* manifold,
							const b2EdgeShape* edgeA, const b2Transform& xfA,
							const b2CircleShape* circleB, const b2Transform& xfB)
{
	manifold->pointCount = 0;

	// Compute circle in frame of edge
	b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));

	b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
	b2Vec2 e = B - A;

	// Barycentric coordinates
	float32 u = b2Dot(e, B - Q);
	float32 v = b2Dot(e, Q - A);

	float32 radius = edgeA->m_radius + circleB->m_radius;

	b2ContactFeature cf;
	cf.indexB = 0;
	cf.typeB = b2ContactFeature::e_vertex;

	// Region A
	if (v <= 0.0f)
	{
		b2Vec2 P = A;
		b2Vec2 d = Q - P;
		float32 dd = b2Dot(d, d);
		if (dd > radius * radius)
		{
			return;
		}

		// Is there an edge connected to A?
		if (edgeA->m_hasVertex0)
		{
			b2Vec2 A1 = edgeA->m_vertex0;
			b2Vec2 B1 = A;
			b2Vec2 e1 = B1 - A1;
			float32 u1 = b2Dot(e1, B1 - Q);

			// Is the circle in Region AB of the previous edge?
			if (u1 > 0.0f)
			{
				return;
			}
		}

		cf.indexA = 0;
		cf.typeA = b2ContactFeature::e_vertex;
		manifold->pointCount = 1;
		manifold->type = b2Manifold::e_circles;
		manifold->localNormal.SetZero();
		manifold->localPoint = P;
		manifold->points[0].id.key = 0;
		manifold->points[0].id.cf = cf;
		manifold->points[0].localPoint = circleB->m_p;
		return;
	}
	
	// Region B
	if (u <= 0.0f)
	{
		b2Vec2 P = B;
		b2Vec2 d = Q - P;
		float32 dd = b2Dot(d, d);
		if (dd > radius * radius)
		{
			return;
		}

		// Is there an edge connected to B?
		if (edgeA->m_hasVertex3)
		{
			b2Vec2 B2 = edgeA->m_vertex3;
			b2Vec2 A2 = B;
			b2Vec2 e2 = B2 - A2;
			float32 v2 = b2Dot(e2, Q - A2);

			// Is the circle in Region AB of the next edge?
			if (v2 > 0.0f)
			{
				return;
			}
		}

		cf.indexA = 1;
		cf.typeA = b2ContactFeature::e_vertex;
		manifold->pointCount = 1;
		manifold->type = b2Manifold::e_circles;
		manifold->localNormal.SetZero();
		manifold->localPoint = P;
		manifold->points[0].id.key = 0;
		manifold->points[0].id.cf = cf;
		manifold->points[0].localPoint = circleB->m_p;
		return;
	}

	// Region AB
	float32 den = b2Dot(e, e);
	b2Assert(den > 0.0f);
	b2Vec2 P = (1.0f / den) * (u * A + v * B);
	b2Vec2 d = Q - P;
	float32 dd = b2Dot(d, d);
	if (dd > radius * radius)
	{
		return;
	}

	b2Vec2 n(-e.y, e.x);
	if (b2Dot(n, Q - A) < 0.0f)
	{
		n.Set(-n.x, -n.y);
	}
	n.Normalize();

	cf.indexA = 0;
	cf.typeA = b2ContactFeature::e_face;
	manifold->pointCount = 1;
	manifold->type = b2Manifold::e_faceA;
	manifold->localNormal = n;
	manifold->localPoint = A;
	manifold->points[0].id.key = 0;
	manifold->points[0].id.cf = cf;
	manifold->points[0].localPoint = circleB->m_p;
}
Exemple #9
0
void test()
{
      cout << " zDate Class Demo \n\n";

      // default constructor, Jan 1 0000
      zDate a;
      cout << a << endl;
      // Various versions of the constructors
      zDate x(zDate::oct,20,1962);
      cout << x << endl;
      // constructor with a julian
      zDate z( 2450000L );
      cout << z << endl;
      // make a date with system date (tests copy constructor)
      zDate s(zDate::Today());
      cout << s << endl;
      // init with the day of year
      zDate y(33, 1996);
      cout << y << endl;
      // init from current system time
      time_t secs_now = time(NULL);
      zDate n(localtime(&secs_now));
      cout << n << endl;

      // using date addition and subtraction
      zDate adder = x + 10;
      cout << adder << endl;
      adder = adder - 25;
      cout << adder << endl;

      //using subtraction of two date objects
      zDate a1(zDate::Today());
      zDate a2 = a1 + 14;
      cout << (a1 - a2) << endl;
      cout << (a2 += 10) << endl;

      a1++;
      cout << "Tommorrow= " << a1 << endl;

      a1 = zDate(zDate::jul, 14, 1991);
      cout << "a1 (7-14-91) < a2 (" << a2
             << ")? ==> " << ((a1 < a2) ? "TRUE" : "FALSE") << endl;
      cout << "a1 (7-14-91) > a2 ("<< a2
             << ")? ==> " << ((a1 > a2) ? "TRUE" : "FALSE") << endl;
      cout << "a1 (7-14-91) < 8-01-91 ? ==> "
             << ((a1 < zDate(zDate::aug, 1, 1991)) ? "TRUE" : "FALSE") << endl;
      cout << "a1 (7-14-91) > 8-01-91 ? ==> "
             << ((a1 > zDate(zDate::aug, 1, 1991)) ? "TRUE" : "FALSE") << endl;
      cout << "a1 (7-14-91) == 7-14-91 ? ==> "
             << ((a1==zDate(zDate::jul, 14, 1991)) ? "TRUE" : "FALSE") << endl;
      zDate a3 = a1;

      cout << "a1 (" << a1 << ") == a3 (" << a3
             << ") ? ==> " << ((a1==a3) ? "TRUE" : "FALSE") << endl;
      zDate a4 = a1;
      ++a4;
      cout << "a1 ("<< a1 <<") == a4 (" << a4
             << ") ? ==> " << ((a1==a4) ? "TRUE" : "FALSE") << endl;

      zDate a5(zDate::Today());
      cout << "Today is: " << a5 << endl;
      a4 = zDate::Today();
      cout << "Today (a4) is: " << a4 << endl;

      cout << "Today + 4 is: " << (a4 += 4) << endl;
      a4 = zDate::Today();
      cout << "Today - 4 is: " << (a4 -= 4) << endl;
      cout << "=========== Leap Year Test ===========\n";
      a1 = zDate(zDate::jan, 15, 1992);
      cout << a1 << "\t" << ((a1.IsLeapYear()) ? "Leap" : "non-Leap");
      cout << "\t" << "day of year:  " << a1.DayOfYear() << endl;

      a1 = zDate(zDate::feb, 16, 1993);
      cout << a1 << "\t" << ((a1.IsLeapYear()) ? "Leap" : "non-Leap");
      cout << "\t" << "day of year:  " << a1.DayOfYear() << endl;

      zDate v4(zDate::Today());
      cout << "---------- Add Stuff -----------\n";
      cout << "Start => " << v4 << endl;
      cout << "Add  4 Weeks  => " << v4.AddWeeks(4) << endl;
      cout << "Sub 52 Weeks  => " << v4.AddWeeks(-52)  << endl;
      cout << "Add  2 Years  => " << v4.AddYears(2)    << endl;

      cout << flush;

      cout << "---------- Misc Stuff -----------\n";
      cout << "The date aboves' day of the month is => " << v4.Day() << endl;
      cout << "There are " << v4.DaysInMonth() << " days in this month.\n";
      cout << "This day happens to be " << v4.DayOfWeek() << " day of week" << endl;
      cout << "on the " << v4.WeekOfYear() << " week of the year," << endl;
      cout << "on the " << v4.WeekOfMonth() << " week of the month, " << endl;
      cout << "which is the "<< (int)v4.Month() << "nth month in the year.\n";
      cout << "The year alone is " << v4.Year() << endl;
      cout << "And this is the " << v4.DayOfYear() << " day of year" << endl;
      cout << "of a year with " << v4.DaysInYear() << " days in it" << endl;
      cout << "which makes exatcly " << v4.WeeksInYear() << " weeks" << endl;

      zDate birthday(zDate::jul, 16, 1973);
      cout << "The age test: i was born on " << birthday
             << " which makes me " << v4.Age(birthday) << " years old" << endl;

      zDate       D2(zDate::jul, 4, 1776);
      int         I1 = 4;

      cout << "Before: I1 = " << I1 << ",  D2 = " << D2 << endl;
      cout << "---------- Postfix '++' test -----------\n";
      cout << "Test : I1++ = " << I1++ << ",  D2++ = " << D2++ << endl;
      cout << "After: I1   = " << I1 << ",  D2   = " << D2 << endl;

      cout << "---------- Prefix '++' test -----------\n";
      cout << "Test : ++I1 = " << ++I1 << ",  ++D2 = " << ++D2 << endl;
      cout << "After:   I1 = " << I1 << ",    D2 = " << D2 << endl;

      cout << "---------- Postfix '--' test -----------\n";
      cout << "Test : I1-- = " << I1-- << ",  D2-- = " << D2-- << endl;
      cout << "After: I1   = " << I1 << ",  D2   = " << D2 << endl;

      cout << "---------- Prefix '--' test -----------\n";
      cout << "Test : --I1 = " << --I1 << ",  --D2 = " << --D2 << endl;
      cout << "After:   I1 = " << I1 << ",    D2 = " << D2 << endl;

      cout << "Last day of this year is dayno "
             << zDate(zDate::dec, 31, 1996).DayOfYear() << endl;
      cout << "Last day of prev year is dayno "
             << zDate(zDate::dec, 31, 1995).DayOfYear() << endl;

      cout << "Today the moon is " << zDate::Today().MoonPhase() << endl;

      zDate today = zDate::Today();

      cout << "DST for " << today.Year() << " starts on " << today.BeginDST()
             << " and ends on " << today.EndDST() << endl;
      cout << "Today, " << today << ", DST is "
             << (today.IsDST() ? "" : "not") << "in effect" << endl;

      zDate date1(zDate::aug, 31, 1996);
      cout << "Adding 6 months to " << date1 << " results in "
             << date1.AddMonths(6) << endl;

      zDate date2(zDate::mar, 31, 1996);
      cout << "Subtracting 1 month from " << date2 << " results in "
             << date2.AddMonths(-1) << endl;

      zDate date3(zDate::jul, 4, 1776);
      cout << "Adding 2400 months to " << date3 << " results in "
             << date3.AddMonths(2400) << endl;

      cout << "Today's day number is " << zDate::Today().DayNumber() << endl;

      zDate date4(zDate::feb, 29, 1996);
      cout << date4 << " subtract two years = " << date4.AddYears(-2) << endl;

      cout << "In 1996, DST began on " << zDate::BeginDST(1996) << endl;

      zDate date5(zDate::sep, 26, 1996);
      cout << "Moon phase on " << date5 << " was " << date5.MoonPhase() << endl;

      zDate date6(zDate::oct, 3, 1996);
      cout << date6 << " + 55 days is " << (date6 + 55) << endl;

      zDate date7(zDate::oct, 4, 1996);
      cout << date7 << " + 217 days is ";
      date7 += 217;
      cout << date7 << endl;
      date7 = zDate(zDate::oct, 4, 1996);
      cout << "Same date - (-217) days is ";
      date7 -= -217;
      cout << date7 << endl;

      cout << "For 1996, Easter is on " << zDate::Easter(1996) << endl;
}
Exemple #10
0
//--------------------Plasticity-------------------------------------
//plasticity integration routine
void DruckerPrager:: plastic_integrator( ) 
{
		bool okay;		// boolean variable to ensure satisfaction of multisurface kuhn tucker conditions
		double f1;
		double f2;
		double norm_eta;
		double Invariant_1;
		double Invariant_ep;
		double norm_ep;
		double norm_dev_ep;
		Vector epsilon_e(6);
		Vector s(6);
		Vector eta(6);
		Vector dev_ep(6);
		Vector Jact(2);

		double fTOL;
		double gTOL;
		fTOL = 0.0;
		gTOL = -1.0e-10;
		
        double NormCep;

		double alpha1;			// hardening parameter for DP surface
		double alpha2;			// hardening parameter for tension cut-off
		Vector n(6);			// normal to the yield surface in strain space
		Vector R(2);			// residual vector
		Vector gamma(2);		// vector of consistency parameters
		Vector dgamma(2);		// incremental vector of consistency parameters
		Matrix g(2,2);			// jacobian of the corner region (return map)
		Matrix g_contra(2,2);	// inverse of jacobian of the corner region

        // set trial state:

        // epsilon_n1_p_trial = ..._n1_p  = ..._n_p
        mEpsilon_n1_p = mEpsilon_n_p;

		// alpha1_n+1_trial
		mAlpha1_n1 = mAlpha1_n;
		// alpha2_n+1_trial
		mAlpha2_n1 = mAlpha2_n;

        // beta_n+1_trial
        mBeta_n1 = mBeta_n;

        // epsilon_elastic = epsilon_n+1 - epsilon_n_p
		epsilon_e = mEpsilon - mEpsilon_n1_p;

        // trial stress
		mSigma = mCe*epsilon_e;

        // deviator stress tensor: s = 2G * IIdev * epsilon_e
        //I1_trial
		Invariant_1 = ( mSigma(0) + mSigma(1) + mSigma(2) );

        // s_n+1_trial
		s = mSigma - (Invariant_1/3.0)*mI1;

        //eta_trial = s_n+1_trial - beta_n;
		eta = s - mBeta_n;
		
		// compute yield function value (contravariant norm)
        norm_eta = sqrt(eta(0)*eta(0) + eta(1)*eta(1) + eta(2)*eta(2) + 2*(eta(3)*eta(3) + eta(4)*eta(4) + eta(5)*eta(5)));

        // f1_n+1_trial
		f1 = norm_eta + mrho*Invariant_1 - root23*Kiso(mAlpha1_n1);

		// f2_n+1_trial
		f2 = Invariant_1 - T(mAlpha2_n1);
		
		// update elastic bulk and shear moduli 
 		this->updateElasticParam();

		// check trial state
		int count = 1;
		if ((f1<=fTOL) && (f2<=fTOL) || mElastFlag < 2) {

			okay = true;
			// trial state = elastic state - don't need to do any updates.
			mCep = mCe;
			count = 0;

			// set state variables for recorders
            Invariant_ep = 	mEpsilon_n1_p(0)+mEpsilon_n1_p(1)+mEpsilon_n1_p(2);

			norm_ep  = sqrt(mEpsilon_n1_p(0)*mEpsilon_n1_p(0) + mEpsilon_n1_p(1)*mEpsilon_n1_p(1) + mEpsilon_n1_p(2)*mEpsilon_n1_p(2)
                           + 0.5*(mEpsilon_n1_p(3)*mEpsilon_n1_p(3) + mEpsilon_n1_p(4)*mEpsilon_n1_p(4) + mEpsilon_n1_p(5)*mEpsilon_n1_p(5)));
			
			dev_ep = mEpsilon_n1_p - one3*Invariant_ep*mI1;

            norm_dev_ep  = sqrt(dev_ep(0)*dev_ep(0) + dev_ep(1)*dev_ep(1) + dev_ep(2)*dev_ep(2)
                           + 0.5*(dev_ep(3)*dev_ep(3) + dev_ep(4)*dev_ep(4) + dev_ep(5)*dev_ep(5)));

			mState(0) = Invariant_1;
			mState(1) = norm_eta;
       		mState(2) = Invariant_ep;
        	mState(3) = norm_dev_ep;
			mState(4) = norm_ep;
			return;
		}
		else {
			// plastic correction required
			okay = false;

			// determine number of active surfaces.  size & fill Jact
			if ( (f1 > fTOL ) && (f2 <= fTOL) ) {
				// f1 surface only
				Jact(0) = 1;
				Jact(1) = 0;
			}
			else if ( (f1 <= fTOL ) && (f2 > fTOL) ) {
				// f2 surface only
				Jact(0) = 0;
				Jact(1) = 1;
			}
			else if ( (f1 > fTOL ) && (f2 > fTOL) ) {
				// both surfaces active
				Jact(0) = 1;
				Jact(1) = 1;
			}
		} 

		//-----------------MultiSurface Placity Return Map--------------------------------------
		while (!okay) {

			alpha1 = mAlpha1_n;
			alpha2 = mAlpha2_n;
	
			//  n = eta / norm_eta;  (contravaraint)
			if (norm_eta < 1.0e-13) {
				n.Zero();
			} else {
				n = eta/norm_eta;
			}
			
			// initialize R, gamma1, gamma2, dgamma1, dgamma2 = 0
			R.Zero();  
			gamma.Zero(); 
			dgamma.Zero();
			// initialize g such that det(g) = 1
			g(0,0) = 1; 
			g(1,1) = 1;
			g(1,0) = 0;
			g(0,1) = 0;

			// Newton procedure to compute nonlinear gamma1 and gamma2
			//initialize terms
			for (int i = 0; i < 2; i++) {
				if (Jact(i) == 1) {
					R(0) = norm_eta - (2*mG + two3*mHprime)*gamma(0) + mrho*Invariant_1 
						   - 9*mK*mrho*mrho_bar*gamma(0) - 9*mK*mrho*gamma(1) - root23*Kiso(alpha1);
					g(0,0) = -2*mG - two3*(mHprime + Kisoprime(alpha1)) - 9*mK*mrho*mrho_bar;
				} else if (Jact(i) == 2) {
					R(1) = Invariant_1 - 9*mK*mrho_bar*gamma(0) - 9*mK*gamma(1) - T(alpha2);
					g(1,1) = -9*mK + mdelta2*T(alpha2);
				}
			}
			if (Jact(0) == 1 && Jact(1) == 1) {
				g(0,1) = -9*mK*mrho;
				g(1,0) = mrho_bar*(-9*mK + mdelta2*T(alpha2));
			} 
			g.Invert(g_contra);

			// iteration counter
			int m = 0;

			//iterate
			while ((fabs(R.Norm()) > 1e-10) && (m < 10)) {

				dgamma = -1*g_contra * R;
				gamma += dgamma;

				//update alpha1 and alpha2
				alpha1 = mAlpha1_n + root23*gamma(0);
				alpha2 = mAlpha2_n + mrho_bar*gamma(0) + gamma(1);

				// reset g & R matrices
				g(0,0) = 1;
				g(1,1) = 1;
				g(1,0) = 0;
				g(0,1) = 0;
				R.Zero();
				for (int i = 0; i < 2; i++) {
				if (Jact(i) == 1) {
					R(0) = norm_eta - (2*mG + two3*mHprime)*gamma(0) + mrho*Invariant_1 
						   - 9*mK*mrho*mrho_bar*gamma(0) - 9*mK*mrho*gamma(1) - root23*Kiso(alpha1);
					g(0,0) = -2*mG - two3*(mHprime + Kisoprime(alpha1)) - 9*mK*mrho*mrho_bar;
				} else if (Jact(i) == 2) {
					R(1) = Invariant_1 - 9*mK*mrho_bar*gamma(0) - 9*mK*gamma(1) - T(alpha2);
					g(1,1) = -9*mK + mdelta2*T(alpha2);
				}
				}
				if (Jact(0) == 1 && Jact(1) == 1) {
					g(0,1) = -9*mK*mrho;
					g(1,0) = mrho_bar*(-9*mK + mdelta2*T(alpha2));
				} 
				g.Invert(g_contra);

				m++;
			}

			// check maintain Kuhn-Tucker conditions
			f1 = norm_eta - (2*mG + two3*mHprime)*gamma(0) + mrho*Invariant_1 
							-9*mK*mrho*mrho_bar*gamma(0) - 9*mK*mrho*gamma(1) - root23*Kiso(alpha1);
			f2 = Invariant_1 - 9*mK*mrho_bar*gamma(0) - 9*mK*gamma(1) - T(alpha2);

			if ( count > 100 ) {
				okay = true;
                break;
			}

			// check active surfaces
			if ((Jact(0) == 1) && (Jact(1) == 0)) {
				// f2 may be > or < f2_tr because of softening of f2 related to alpha1
				if (f2 >= fTOL) {
					// okay = false;
					Jact(0) = 1;
					Jact(1) = 1;
					count += 1;

				} else {
					okay = true;

				}
			} else if ((Jact(0) == 0) && (Jact(1) == 1)) {
				// f1 will always be less than f1_tr
				okay = true;
			} else if ((Jact(0) == 1) && (Jact(1) == 1)) {
				if ((gamma(0) <= gTOL) && (gamma(1) > gTOL)){
					// okay = false;
					Jact(0) = 0;
					Jact(1) = 1;
					count += 1;
				} else if ((gamma(0) > gTOL) && (gamma(1) <= gTOL)){
					// okay = false;
					Jact(0) = 1;
					Jact(1) = 0;
					count += 1;
				} else if ((gamma(0) > gTOL) && (gamma(1) > gTOL)) {
					okay = true;
				}
			}
							
			if ( (count > 3) && (!okay) ) {
				Jact(0) = 1;
				Jact(1) = 1;
				count += 100;
			}

			if ( count > 3 ) {
				opserr << "Jact = " << Jact;
				opserr << "count = " << count << endln;
			}

		} // end of while(!okay) loop


		//update everything and exit!

		Vector b1(6);
		Vector b2(6);
		Vector n_covar(6);
		Vector temp1(6);
		Vector temp2(6);

		// update alpha1 and alpha2
		mAlpha1_n1 = alpha1;
		mAlpha2_n1 = alpha2;

		//update epsilon_n1_p
		//first calculate n_covar
		// n_a = G_ab * n^b = covariant
		n_covar(0) = n(0);
		n_covar(1) = n(1);
		n_covar(2) = n(2);
		n_covar(3) = 2*n(3);
		n_covar(4) = 2*n(4);
		n_covar(5) = 2*n(5);
		mEpsilon_n1_p = mEpsilon_n_p + (mrho_bar*gamma(0) + gamma(1))*mI1 + gamma(0)*n_covar;

           
        Invariant_ep = 	mEpsilon_n1_p(0)+mEpsilon_n1_p(1)+mEpsilon_n1_p(2);

		norm_ep  = sqrt(mEpsilon_n1_p(0)*mEpsilon_n1_p(0) + mEpsilon_n1_p(1)*mEpsilon_n1_p(1) + mEpsilon_n1_p(2)*mEpsilon_n1_p(2)
                           + 0.5*(mEpsilon_n1_p(3)*mEpsilon_n1_p(3) + mEpsilon_n1_p(4)*mEpsilon_n1_p(4) + mEpsilon_n1_p(5)*mEpsilon_n1_p(5)));
			
		dev_ep = mEpsilon_n1_p - one3*Invariant_ep*mI1;

        norm_dev_ep  = sqrt(dev_ep(0)*dev_ep(0) + dev_ep(1)*dev_ep(1) + dev_ep(2)*dev_ep(2)
                     + 0.5*(dev_ep(3)*dev_ep(3) + dev_ep(4)*dev_ep(4) + dev_ep(5)*dev_ep(5)));

		// update sigma
		mSigma -= (3*mK*mrho_bar*gamma(0) + 3*mK*gamma(1))*mI1 + 2*mG*gamma(0)*n;

		s            -= 2*mG*gamma(0) * n;
		Invariant_1  -= 9*mK*mrho_bar*gamma(0) + 9*mK*gamma(1);
		//mSigma        = s + Invariant_1/3.0 * mI1;

		//update beta_n1
		mBeta_n1 = mBeta_n - (two3*mHprime*gamma(0))*n;

		//eta_n+1 = s_n+1 - beta_n+1;
		eta = s - mBeta_n1;
        norm_eta = sqrt(eta(0)*eta(0) + eta(1)*eta(1) + eta(2)*eta(2) + 2*(eta(3)*eta(3) + eta(4)*eta(4) + eta(5)*eta(5)));
			
		// update Cep
		// note: Cep is contravariant
		if ((Jact(0) == 1) && (Jact(1) == 0)) {
			b1 = 2*mG*n + 3*mK*mrho*mI1;
			b2.Zero();
		} else if ((Jact(0) == 0) && (Jact(1) == 1)){
			b1.Zero();
			b2 = 3*mK*mI1;
		} else if ((Jact(0) == 1) && (Jact(1) == 1)){
			b1 = 2*mG*n + 3*mK*mrho*mI1;
			b2 = 3*mK*mI1;
		}

		temp1 = g_contra(0,0)*b1 + g_contra(0,1)*b2;  
		temp2 = mrho_bar*temp1 + g_contra(1,0)*b1 + g_contra(1,1)*b2;

		NormCep = 0.0;
		for (int i = 0; i < 6; i++){
			for (int j = 0; j < 6; j++) {
				mCep(i,j) = mCe(i,j)
						  + 3*mK * mI1(i)*temp2(j)  
						  + 2*mG * n(i)*temp1(j)
						  - 4*mG*mG/norm_eta*gamma(0) * (mIIdev(i,j) - n(i)*n(j));
				NormCep += mCep(i,j)*mCep(i,j);
			}
		}

		if ( NormCep < 1e-10){
			mCep = 1.0e-3 * mCe;
			opserr << "NormCep = " << NormCep << endln;
		}

		mState(0) = Invariant_1;
		mState(1) = norm_eta;
        mState(2) = Invariant_ep;
        mState(3) = norm_dev_ep;
		mState(4) = norm_ep;

	return;
}
Exemple #11
0
int main(int argc, char *argv[]) {
    try {
        utility::thread_number_range threads(get_default_num_threads);
        utility::parse_cli_arguments(argc, argv,
                                     utility::cli_argument_pack()
                                     //"-h" option for for displaying help is present implicitly
                                     .positional_arg(threads,"#threads","  number of threads to use; a range of the "
                                                     "form low[:high]\n              where low and optional high are "
                                                     "non-negative integers,\n              or 'auto' for the TBB "
                                                     "default")
                                     .arg(verbose,"verbose","   print diagnostic output to screen")
                                     .arg(silent,"silent","    limits output to timing info; overrides verbose")
        );

        if (silent) verbose = false;  // make silent override verbose

        tick_count start = tick_count::now();
        for(int p = threads.first; p <= threads.last; ++p ) {
            task_scheduler_init init(p);
            if (!silent)  cout << "graph test running on " << p << " threads.\n";
            
            graph g;

            { // test buffer: 0, 1
                buffer b(g);
                toggle input(g);
                led output(g, "OUTPUT", false); // false means we will explicitly call display to see LED
                
                make_edge(input.get_out(), b.get_in(0));
                make_edge(b.get_out(), output.get_in());
                
                if (!silent) printf("Testing buffer...\n");
                input.activate(); // 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == low);
                input.flip(); // 1
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
            }

            { // test not_gate: 0, 1
                not_gate n(g);
                toggle input(g);
                led output(g, "OUTPUT", false);
                
                make_edge(input.get_out(), n.get_in(0));
                make_edge(n.get_out(), output.get_in());
                
                if (!silent) printf("Testing not_gate...\n");
                input.activate(); // 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
                input.flip(); // 1
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == low);
            }

            { // test two-input and_gate: 00, 01, 10, 11
                and_gate<two_input> a(g);
                toggle input0(g);
                toggle input1(g);
                led output(g, "OUTPUT", false);
                
                make_edge(input0.get_out(), a.get_in(0));
                make_edge(input1.get_out(), a.get_in(1));
                make_edge(a.get_out(), output.get_in());
                
                if (!silent) printf("Testing and_gate...\n");
                input1.activate();  input0.activate();  // 0 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == low);
                input0.flip();  // 0 1
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == low);
                input1.flip(); input0.flip();  // 1 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == low);
                input0.flip();  // 1 1
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
            }

            { // test three-input or_gate: 000, 001, 010, 100, 011, 101, 110, 111
                or_gate<three_input> o(g);
                toggle input0(g);
                toggle input1(g);
                toggle input2(g);
                led output(g, "OUTPUT", false);
                
                make_edge(input0.get_out(), o.get_in(0));
                make_edge(input1.get_out(), o.get_in(1));
                make_edge(input2.get_out(), o.get_in(2));
                make_edge(o.get_out(), output.get_in());
                
                if (!silent) printf("Testing or_gate...\n");
                input2.activate();  input1.activate();  input0.activate();  // 0 0 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == low);
                input0.flip();  // 0 0 1
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
                input1.flip(); input0.flip();  // 0 1 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
                input2.flip();  input1.flip();  // 1 0 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
                input2.flip();  input1.flip();  input0.flip();  // 0 1 1
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
                input2.flip();  input1.flip();  // 1 0 1
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
                input1.flip();  input0.flip();  // 1 1 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
                input0.flip();  // 1 1 1
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
            }

            { // test two-input xor_gate: 00, 01, 10, 11
                xor_gate<two_input> x(g);
                toggle input0(g);
                toggle input1(g);
                led output(g, "OUTPUT", false);
                
                make_edge(input0.get_out(), x.get_in(0));
                make_edge(input1.get_out(), x.get_in(1));
                make_edge(x.get_out(), output.get_in());
                
                if (!silent) printf("Testing xor_gate...\n");
                input1.activate();  input0.activate();  // 0 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == low);
                input0.flip();  // 0 1
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
                input1.flip();  input0.flip();  // 1 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
                input0.flip();  // 1 1
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == low);
            }


            { // test two-input nor_gate: 00, 01, 10, 11
                nor_gate<two_input> n(g);
                toggle input0(g);
                toggle input1(g);
                led output(g, "OUTPUT", false);
                
                make_edge(input0.get_out(), n.get_in(0));
                make_edge(input1.get_out(), n.get_in(1));
                make_edge(n.get_out(), output.get_in());
                
                if (!silent) printf("Testing nor_gate...\n");
                input1.activate();  input0.activate();  // 0 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == high);
                input0.flip();  // 0 1
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == low);
                input1.flip();  input0.flip();  // 1 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == low);
                input0.flip();  // 1 1
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == low);
            }

            { // test steady_signal and digit
                steady_signal input0(g, high);
                steady_signal input1(g, low);
                and_gate<two_input> a(g);
                or_gate<two_input> o(g);
                xor_gate<two_input> x(g);
                nor_gate<two_input> n(g);
                digit output(g, "OUTPUT", false);
                
                make_edge(input0.get_out(), a.get_in(0));
                make_edge(input1.get_out(), a.get_in(1));
                make_edge(a.get_out(), output.get_in(0));

                make_edge(input0.get_out(), o.get_in(0));
                make_edge(input1.get_out(), o.get_in(1));
                make_edge(o.get_out(), output.get_in(1));

                make_edge(input0.get_out(), x.get_in(0));
                make_edge(input1.get_out(), x.get_in(1));
                make_edge(x.get_out(), output.get_in(2));

                make_edge(input0.get_out(), n.get_in(0));
                make_edge(input1.get_out(), n.get_in(1));
                make_edge(n.get_out(), output.get_in(3));
                
                if (!silent) printf("Testing steady_signal...\n");
                input0.activate();  // 1
                input1.activate();  // 0
                g.wait_for_all();
                if (!silent) output.display();
                assert(output.get_value() == 6);
            }

            { // test push_button
                push_button p(g);
                buffer b(g);
                led output(g, "OUTPUT", !silent); // true means print all LED state changes

                make_edge(p.get_out(), b.get_in(0));
                make_edge(b.get_out(), output.get_in());

                if (!silent) printf("Testing push_button...\n");
                p.press();
                p.release();
                p.press();
                p.release();
                g.wait_for_all();
            }

            { // test one_bit_adder
                one_bit_adder my_adder(g);
                toggle A(g);
                toggle B(g);
                toggle CarryIN(g);
                led Sum(g, "SUM");
                led CarryOUT(g, "CarryOUT");
                
                make_edge(A.get_out(), my_adder.get_A());
                make_edge(B.get_out(), my_adder.get_B());
                make_edge(CarryIN.get_out(), my_adder.get_CI());
                make_edge(my_adder.get_out(), Sum.get_in());
                make_edge(my_adder.get_CO(), CarryOUT.get_in());
                
                A.activate();
                B.activate();
                CarryIN.activate();
                
                if (!silent) printf("A on\n");
                A.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == high) && (CarryOUT.get_value() == low));
                
                if (!silent) printf("A off\n");
                A.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == low) && (CarryOUT.get_value() == low));
                
                if (!silent) printf("B on\n");
                B.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == high) && (CarryOUT.get_value() == low));
                if (!silent) printf("B off\n");
                B.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == low) && (CarryOUT.get_value() == low));
                
                if (!silent) printf("CarryIN on\n");
                CarryIN.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == high) && (CarryOUT.get_value() == low));
                if (!silent) printf("CarryIN off\n");
                CarryIN.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == low) && (CarryOUT.get_value() == low));
                
                if (!silent) printf("A&B on\n");
                A.flip();
                B.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == low) && (CarryOUT.get_value() == high));
                if (!silent) printf("A&B off\n");
                A.flip();
                B.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == low) && (CarryOUT.get_value() == low));
                
                if (!silent) printf("A&CarryIN on\n");
                A.flip();
                CarryIN.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == low) && (CarryOUT.get_value() == high));
                if (!silent) printf("A&CarryIN off\n");
                A.flip();
                CarryIN.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == low) && (CarryOUT.get_value() == low));
                
                if (!silent) printf("B&CarryIN on\n");
                B.flip();
                CarryIN.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == low) && (CarryOUT.get_value() == high));
                if (!silent) printf("B&CarryIN off\n");
                B.flip();
                CarryIN.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == low) && (CarryOUT.get_value() == low));
                
                if (!silent) printf("A&B&CarryIN on\n");
                A.flip();
                B.flip();
                CarryIN.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == high) && (CarryOUT.get_value() == high));
                if (!silent) printf("A&B&CarryIN off\n");
                A.flip();
                B.flip();
                CarryIN.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == low) && (CarryOUT.get_value() == low));
            }

            { // test four_bit_adder
                four_bit_adder four_adder(g);
                std::vector<toggle> A(4, toggle(g));
                std::vector<toggle> B(4, toggle(g));
                toggle CarryIN(g);
                digit Sum(g, "SUM");
                led CarryOUT(g, "CarryOUT");
                
                for (int i=0; i<4; ++i) {
                    make_edge(A[i].get_out(), four_adder.get_A(i));
                    make_edge(B[i].get_out(), four_adder.get_B(i));
                    make_edge(four_adder.get_out(i), Sum.get_in(i));
                }
                make_edge(CarryIN.get_out(), four_adder.get_CI());
                make_edge(four_adder.get_CO(), CarryOUT.get_in());
                
                // Activate all switches at low state
                for (int i=0; i<4; ++i) {
                    A[i].activate();
                    B[i].activate();
                }
                CarryIN.activate();
                
                if (!silent) printf("1+0\n");
                A[0].flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == 1) && (CarryOUT.get_value() == low));
                
                if (!silent) printf("0+1\n");
                A[0].flip();
                B[0].flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == 1) && (CarryOUT.get_value() == low));
                
                if (!silent) printf("3+4\n");
                A[0].flip();
                A[1].flip();
                B[0].flip();
                B[2].flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == 7) && (CarryOUT.get_value() == low));
                
                if (!silent) printf("6+1\n");
                A[0].flip();
                A[2].flip();
                B[0].flip();
                B[2].flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == 7) && (CarryOUT.get_value() == low));
                
                if (!silent) printf("0+0+carry\n");
                A[1].flip();
                A[2].flip();
                B[0].flip();
                CarryIN.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == 1) && (CarryOUT.get_value() == low));
                
                if (!silent) printf("15+15+carry\n");
                A[0].flip();
                A[1].flip();
                A[2].flip();
                A[3].flip();
                B[0].flip();
                B[1].flip();
                B[2].flip();
                B[3].flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == 0xf) && (CarryOUT.get_value() == high));
                
                if (!silent) printf("8+8\n");
                A[0].flip();
                A[1].flip();
                A[2].flip();
                B[0].flip();
                B[1].flip();
                B[2].flip();
                CarryIN.flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == 0) && (CarryOUT.get_value() == high));
                
                if (!silent) printf("0+0\n");
                A[3].flip();
                B[3].flip();
                g.wait_for_all();
                if (!silent) Sum.display();
                if (!silent) CarryOUT.display();
                assert((Sum.get_value() == 0) && (CarryOUT.get_value() == low));
            }

            { // test D_latch
                D_latch my_d_latch(g);
                toggle D(g);
                pulse E(g, 500, 4); // clock changes every 500ms; stops after 4 changes
                led Q(g, " Q", verbose); // if true, LEDs print at every state change
                led notQ(g, "~Q", verbose);
                
                make_edge(D.get_out(), my_d_latch.get_D());
                make_edge(E.get_out(), my_d_latch.get_E());
                make_edge(my_d_latch.get_Q(), Q.get_in());
                make_edge(my_d_latch.get_notQ(), notQ.get_in());
                
                D.activate();
                
                if (!silent) printf("Toggling D\n");
                E.activate();
                D.flip();
                g.wait_for_all();
                if (!silent && !verbose) { Q.display(); notQ.display(); }
                assert((Q.get_value() == high) && (notQ.get_value() == low));
                E.reset();
                
                if (!silent) printf("Toggling D\n");
                E.activate();
                D.flip();
                g.wait_for_all();
                if (!silent && !verbose) { Q.display(); notQ.display(); }
                assert((Q.get_value() == low) && (notQ.get_value() == high));
                E.reset();
                
                if (!silent) printf("Toggling D\n");
                E.activate();
                D.flip();
                g.wait_for_all();
                if (!silent && !verbose) { Q.display(); notQ.display(); }
                assert((Q.get_value() == high) && (notQ.get_value() == low));
                E.reset();
                
                if (!silent) printf("Toggling D\n");
                E.activate();
                D.flip();
                g.wait_for_all();
                if (!silent && !verbose) { Q.display(); notQ.display(); }
                assert((Q.get_value() == low) && (notQ.get_value() == high));
                E.reset();
                
                if (!silent) printf("Toggling D\n");
                E.activate();
                D.flip();
                g.wait_for_all();
                if (!silent && !verbose) { Q.display(); notQ.display(); }
                assert((Q.get_value() == high) && (notQ.get_value() == low));
            }
        }
        utility::report_elapsed_time((tbb::tick_count::now() - start).seconds());
        return 0;
    } catch(std::exception& e) {
        cerr<<"error occurred. error text is :\"" <<e.what()<<"\"\n";
        return 1;
    }
}
Exemple #12
0
 template <> void Vector<int>::qSortStd(){
   qsort((char*)memory(),n(),sizeof(int),compareInt) ;
 }
Exemple #13
0
int main(int argc, char **argv)
{

  /* laser data */
  LMS1xx laser;
  scanCfg cfg;
  scanDataCfg dataCfg;
  scanData data;

  /* published data */
  sensor_msgs::LaserScan scan_msg;

  /* parameters */
  std::string host;
  std::string topic_id;
  std::string frame_id;
  bool invert;

  /* variables */
  int retrycount = 0;

  ros::init(argc, argv, "LMS111_node");
  ros::NodeHandle nh;
  ros::NodeHandle n("~");

  n.param<std::string> ("host", host, "192.168.0.10");
  n.param<std::string> ("topic_id", topic_id, "scan_msg");
  n.param<std::string> ("frame_id", frame_id, "/map");
  n.param<bool> ("invert_output",invert,false);

  ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan> (topic_id, 1);

  ROS_INFO("Connecting to device at : %s", host.c_str());

  /* initialize hardware */
  laser.connect(host);

  if (laser.isConnected())
  {

    ROS_INFO("Connected to device at : %s", host.c_str());

    laser.login();
    cfg = laser.getScanCfg();

    scan_msg.header.frame_id = frame_id;

    scan_msg.range_min = 0.01;
    scan_msg.range_max = 6.0;

    scan_msg.scan_time = 100.0 / cfg.scaningFrequency;

    scan_msg.angle_increment = cfg.angleResolution / 10000.0 * DEG2RAD;
    scan_msg.angle_min = cfg.startAngle / 10000.0 * DEG2RAD - M_PI / 2;
    scan_msg.angle_max = cfg.stopAngle / 10000.0 * DEG2RAD - M_PI / 2;

    //std::cout << "res : " << cfg.angleResolution << " freq : " << cfg.scaningFrequency << std::endl;

    int num_values;
    if (cfg.angleResolution == 2500)
    {
      num_values = 1081;
    }
    else if (cfg.angleResolution == 5000)
    {
      num_values = 541;
    }
    else
    {
      ROS_ERROR("Unsupported resolution");
      return 0;
    }

    scan_msg.time_increment = scan_msg.scan_time / num_values;

    scan_msg.ranges.resize(num_values);
    scan_msg.intensities.resize(num_values);

    dataCfg.outputChannel = 1;
    dataCfg.remission = true;
    dataCfg.resolution = 1;
    dataCfg.encoder = 0;
    dataCfg.position = false;
    dataCfg.deviceName = false;
    dataCfg.outputInterval = 1;

    laser.setScanDataCfg(dataCfg);

    laser.startMeas();

    status_t stat;
    do // wait for ready status
    {
      retrycount++;
      stat = laser.queryStatus();
      ros::Duration(1.0).sleep();
    } while ((stat != ready_for_measurement) && retrycount < MAXRETRYES);

    if (stat == ready_for_measurement)
    {
      ROS_INFO("Device ready - receiving measurement");
      laser.scanContinous(1);

      while (ros::ok())
      {
        ros::Time start = ros::Time::now();

        scan_msg.header.stamp = start;
        ++scan_msg.header.seq;

        laser.getData(data);

        if(invert == false)
        {
			for (int i = 0; i < data.dist_len1; i++)
			{
			  scan_msg.ranges[i] = data.dist1[i] * 0.001;
			}

			for (int i = 0; i < data.rssi_len1; i++)
			{
			  scan_msg.intensities[i] = data.rssi1[i];
			}
        }
        else
        {
			for (int i = 0; i < data.dist_len1; i++)
			{
			  scan_msg.ranges[i] = data.dist1[data.dist_len1 - i - 1] * 0.001;
			}

			for (int i = 0; i < data.rssi_len1; i++)
			{
			  scan_msg.intensities[i] = data.rssi1[data.rssi_len1 - i- 1];
			}
        }

        scan_pub.publish(scan_msg);

        ros::spinOnce();
      }

      laser.scanContinous(0);
      laser.stopMeas();
      laser.disconnect();
    }
    else
    {
      ROS_ERROR("Device not ready for measurement");
      return 0;
    }
  }
  else
  {
    ROS_ERROR("Connection to device @ %s failed", host.c_str());
  }
  return 0;

}
  void runWriteIncrementalTest()
  {
    stringstream ss(stringstream::out);

    PbfWriter writer;
    // makes for a consistent output.
    writer.setCompressionLevel(0);
    writer.includVersion(false);

    OsmMap::resetCounters();
    srand(0);

    shared_ptr<OsmMap> map(new OsmMap());
    shared_ptr<Node> n(new Node(Status::Unknown1, 72, 42.0, 3.14159, 7.1));
    n->setTag("hello", "world");
    n->setTag("note", "test tag");
    map->addNode(n);
    shared_ptr<Way> w(new Way(Status::Unknown1, 42, 1.7));
    vector<long> nodes;
    nodes.push_back(1);
    nodes.push_back(3);
    nodes.push_back(5);
    nodes.push_back(7);
    nodes.push_back(11);
    w->addNodes(nodes);
    w->setTag("hello", "world");
    w->setTag("highway", "road");
    w->setTag("note", "test tag");
    map->addWay(w);
    writer.intializePartial(&ss);
    writer.writePartial(map);

    map->clear();

    n.reset(new Node(Status::Unknown1, 73, 20, 30, 15));
    map->addNode(n);
    w.reset(new Way(Status::Unknown1, 43, 1.7));
    nodes.clear();
    nodes.push_back(73);
    w->addNodes(nodes);
    map->addWay(w);

    writer.writePartial(map);

    writer.finalizePartial();

    //_dumpString(ss.str());

    const unsigned char data[] = {
        0,   0,   0,  13,  10,   9,  79,  83,  77,  72, 101,  97, 100, 101, 114,  24,  56,  16,
       41,  26,  52, 120,   1,   1,  41,   0, 214, 255,  34,  14,  79, 115, 109,  83,  99, 104,
      101, 109,  97,  45,  86,  48,  46,  54,  34,  10,  68, 101, 110, 115, 101,  78, 111, 100,
      101, 115, 130,   1,  10,  72, 111, 111, 116, 101, 110,  97, 110, 110, 121,   9, 172,  13,
      140,   0,   0,   0,  12,  10,   7,  79,  83,  77,  68,  97, 116,  97,  24, 221,   1,  16,
      204,   1,  26, 215,   1, 120,   1,   1, 204,   0,  51, 255,  10,  89,  10,   0,  10,   4,
      110, 111, 116, 101,  10,   8, 116, 101, 115, 116,  32, 116,  97, 103,  10,   5, 104, 101,
      108, 108, 111,  10,   5, 119, 111, 114, 108, 100,  10,  14, 101, 114, 114, 111, 114,  58,
       99, 105, 114,  99, 117, 108,  97, 114,  10,   3,  55,  46,  49,  10,  11, 104, 111, 111,
      116,  58, 115, 116,  97, 116, 117, 115,  10,   1,  49,  10,   7, 104, 105, 103, 104, 119,
       97, 121,  10,   4, 114, 111,  97, 100,  10,   3,  49,  46,  55,  18,  42,  18,  40,  10,
        3, 144,   1,   2,  66,   9, 184, 249, 250,  29, 200, 146, 146, 128,   2,  74,  10, 128,
      196, 197, 144,   3, 255, 187, 231, 209,   1,  82,  10,   1,   2,   3,   4,   5,   6,   7,
        8,   0,   0,  18,  67,  26,  23,   8,  42,  18,   5,   1,   3,   9,   5,   7,  26,   5,
        2,   4,  10,  11,   8,  66,   5,   2,   4,   4,   4,   8,  26,  40,   8,  43,  18,   2,
        5,   7,  26,   2,  11,   8,  66,   2, 146,   1,  74,  24,   8, 128, 136, 222, 190,   1,
       16, 128, 136, 222, 190,   1,  24, 128, 140, 141, 158,   2,  32, 128, 140, 141, 158,   2,
      109,  93,  53, 122};
    size_t dataSize = 310;

    CPPUNIT_ASSERT_EQUAL(dataSize, ss.str().size());
    for (size_t i = 0; i < dataSize; i++)
    {
      CPPUNIT_ASSERT_EQUAL((unsigned int)data[i], (unsigned int)(unsigned char)ss.str().at(i));
    }
  }
Exemple #15
0
int w(int year,int month,int day)
{
  /*w=(N-621049)%7(0<=w<7)*/
	return(int)((n(year,month,day)%7-621049L%7+7)%7);
}
int main(int argc, char **argv) {
  const int d = 5;       // expected-note {{'d' defined here}}
  const int da[5] = {0}; // expected-note {{'da' defined here}}
  S4 e(4);
  S5 g(5);
  S3 m;
  S6 n(2);
  int i;
  int &j = i;
#pragma omp target parallel for simd lastprivate // expected-error {{expected '(' after 'lastprivate'}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate( // expected-error {{expected expression}} expected-error {{expected ')'}} expected-note {{to match this '('}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate() // expected-error {{expected expression}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(argc // expected-error {{expected ')'}} expected-note {{to match this '('}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(argc, // expected-error {{expected expression}} expected-error {{expected ')'}} expected-note {{to match this '('}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(argc > 0 ? argv[1] : argv[2]) // expected-error {{expected variable name}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(argc)
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(S1) // expected-error {{'S1' does not refer to a value}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(a, b, c, d, f) // expected-error {{lastprivate variable with incomplete type 'S1'}} expected-error 1 {{const-qualified variable without mutable fields cannot be lastprivate}} expected-error 2 {{const-qualified variable cannot be lastprivate}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(argv[1]) // expected-error {{expected variable name}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(2 * 2) // expected-error {{expected variable name}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(ba)
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(ca) // expected-error {{const-qualified variable without mutable fields cannot be lastprivate}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(da) // expected-error {{const-qualified variable cannot be lastprivate}}
  for (i = 0; i < argc; ++i)
    foo();
  int xa;
#pragma omp target parallel for simd lastprivate(xa) // OK
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(S2::S2s) // expected-error {{shared variable cannot be lastprivate}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(S2::S2sc) // expected-error {{const-qualified variable cannot be lastprivate}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd safelen(5) // OK
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(e, g) // expected-error {{calling a private constructor of class 'S4'}} expected-error {{calling a private constructor of class 'S5'}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(m) // expected-error {{'operator=' is a private member of 'S3'}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(h, B::x) // expected-error 2 {{threadprivate or thread local variable cannot be lastprivate}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd private(xa), lastprivate(xa) // expected-error {{private variable cannot be lastprivate}} expected-note {{defined as private}}
  for (i = 0; i < argc; ++i)
    foo();
// expected-note@+2 {{defined as lastprivate}}
// expected-error@+2 {{loop iteration variable in the associated loop of 'omp target parallel for simd' directive may not be lastprivate, predetermined as linear}}
#pragma omp target parallel for simd lastprivate(i)
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp parallel private(xa)
#pragma omp target parallel for simd lastprivate(xa)
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp parallel reduction(+ : xa)
#pragma omp target parallel for simd lastprivate(xa)
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(j)
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd firstprivate(m) lastprivate(m) // expected-error {{'operator=' is a private member of 'S3'}}
  for (i = 0; i < argc; ++i)
    foo();
#pragma omp target parallel for simd lastprivate(n) firstprivate(n) // OK
  for (i = 0; i < argc; ++i)
    foo();
  static int si;
#pragma omp target parallel for simd lastprivate(si) // OK
  for (i = 0; i < argc; ++i)
    si = i + 2;

  return foomain<S4, S5>(argc, argv); // expected-note {{in instantiation of function template specialization 'foomain<S4, S5>' requested here}}
}
Exemple #17
0
 NamespaceDetails* NamespaceIndex::details(const StringData& ns) {
     Namespace n(ns);
     return details(n);
 }
Exemple #18
0
 AltCell & operator=( const AltCell & orig ){
   AltCell n(orig);
   std::swap(p, n.p);
   std::swap(alt, n.alt);
   return (*this);
 }
int main(int argc, char **argv) {
    const int d = 5;       // expected-note {{constant variable is predetermined as shared}}
    const int da[5] = {0}; // expected-note {{constant variable is predetermined as shared}}
    S4 e(4);
    S5 g(5);
    S3 m;
    S6 n(2);
    int i;
    int &j = i;
    #pragma omp parallel
    #pragma omp taskloop lastprivate // expected-error {{expected '(' after 'lastprivate'}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate( // expected-error {{expected expression}} expected-error {{expected ')'}} expected-note {{to match this '('}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate() // expected-error {{expected expression}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(argc // expected-error {{expected ')'}} expected-note {{to match this '('}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(argc, // expected-error {{expected expression}} expected-error {{expected ')'}} expected-note {{to match this '('}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(argc > 0 ? argv[1] : argv[2]) // expected-error {{expected variable name}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(argc)
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(S1) // expected-error {{'S1' does not refer to a value}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(a, b, c, d, f) // expected-error {{lastprivate variable with incomplete type 'S1'}} expected-error 3 {{shared variable cannot be lastprivate}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(argv[1]) // expected-error {{expected variable name}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(2 * 2) // expected-error {{expected variable name}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(ba)
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(ca) // expected-error {{shared variable cannot be lastprivate}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(da) // expected-error {{shared variable cannot be lastprivate}}
    for (i = 0; i < argc; ++i)
        foo();
    int xa;
    #pragma omp parallel
    #pragma omp taskloop lastprivate(xa) // OK
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(S2::S2s)
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(S2::S2sc) // expected-error {{shared variable cannot be lastprivate}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop safelen(5) // expected-error {{unexpected OpenMP clause 'safelen' in directive '#pragma omp taskloop'}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(e, g) // expected-error {{calling a private constructor of class 'S4'}} expected-error {{calling a private constructor of class 'S5'}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(m) // expected-error {{'operator=' is a private member of 'S3'}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(h) // expected-error {{threadprivate or thread local variable cannot be lastprivate}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(B::x) // expected-error {{threadprivate or thread local variable cannot be lastprivate}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop private(xa), lastprivate(xa) // expected-error {{private variable cannot be lastprivate}} expected-note {{defined as private}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(i)
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel private(xa)
    #pragma omp taskloop lastprivate(xa)
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel reduction(+ : xa)
    #pragma omp taskloop lastprivate(xa)
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(j)
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop firstprivate(m) lastprivate(m) // expected-error {{'operator=' is a private member of 'S3'}}
    for (i = 0; i < argc; ++i)
        foo();
    #pragma omp parallel
    #pragma omp taskloop lastprivate(n) firstprivate(n) // OK
    for (i = 0; i < argc; ++i)
        foo();
    static int si;
    #pragma omp taskloop lastprivate(si) // OK
    for (i = 0; i < argc; ++i)
        si = i + 1;
    return foomain<S4, S5>(argc, argv); // expected-note {{in instantiation of function template specialization 'foomain<S4, S5>' requested here}}
}
Exemple #20
0
    void ViewingArea::update_projection_matrices()
    {
      // if the inset is asymmetric, shift the viewing volume accordingly
      GLfloat xShift((insetCenter_[X_INDEX]-centerAndRelative_[X_INDEX])/centerAndRelative_[X_INDEX]);
      GLfloat yShift((insetCenter_[Y_INDEX]-centerAndRelative_[Y_INDEX])/centerAndRelative_[Y_INDEX]);
	  
      // if the inset is smaller than the window, rescale the viewing volume accordingly;
      GLfloat xScale((insetRelative_[X_INDEX]/centerAndRelative_[X_INDEX]));
      GLfloat yScale((insetRelative_[Y_INDEX]/centerAndRelative_[Y_INDEX]));
      
      // correct for the aspect ratio of the inset; 
      GLfloat r(insetRelative_[X_INDEX]/insetRelative_[Y_INDEX]);
      if(r>1)
	xScale/=r;
      else
	yScale*=r;

      // check if we have orthogonal projection
      if(!(angle_>0))
	{
	  projectionMatrix_[X_INDEX+N_ROWS*X_INDEX]=xScale;inverseProjectionMatrix_[X_INDEX+N_ROWS*X_INDEX]=1/xScale;
	  projectionMatrix_[Y_INDEX+N_ROWS*Y_INDEX]=yScale;inverseProjectionMatrix_[Y_INDEX+N_ROWS*Y_INDEX]=1/yScale;
	  projectionMatrix_[Z_INDEX+N_ROWS*Z_INDEX]=-zScaleOrtho_;inverseProjectionMatrix_[Z_INDEX+N_ROWS*Z_INDEX]=-1/zScaleOrtho_;
	  
	  projectionMatrix_[X_INDEX+N_ROWS*N_SPATIAL_DIMENSIONS]=xShift;inverseProjectionMatrix_[X_INDEX+N_ROWS*N_SPATIAL_DIMENSIONS]=-xShift/xScale;
	  projectionMatrix_[Y_INDEX+N_ROWS*N_SPATIAL_DIMENSIONS]=yShift;inverseProjectionMatrix_[Y_INDEX+N_ROWS*N_SPATIAL_DIMENSIONS]=-yShift/yScale;
	}
      else // perspective projection
	{
	  /* The original (-1,-1,-1) - (1,1,1) viewing volume must be rescaled to fit into
	     the window depending on its distance from the camera. 
	  */
	  xScale*=(zShift_+zCamera_);
	  yScale*=(zShift_+zCamera_);

	  // set near and far clipping planes (magic numbers right now -> find better way)
	  GLfloat n(zShift_/10);
	  GLfloat f(zShift_*10);

	  /* put together the perspective projection everything together in this order:
	     scale x, y and z in camera space,
	     shift z in camera space,
	     apply perspective transformation (as in gluPerspective, aspect ratio is
	     already built into xScale and yScale)
	     shift x and y in screen space (shifting in camera space would decenter the frustum)
	     the projection matrix below is the product of all these transformations
	  */
	  
	  /* note that zShift_ is positive, with larger values equivalent to a larger distance 
	     from the viewer. In camera space, however, negative values are further away from 
	     the viewer. zShift_ enters the projection matrix below such that a large positive
	     value of zShift_ will create a large negative shift in the z coordinate of the input
	     vector.
	  */
	  
	  a_=(-(f+n)/(f-n));
	  b_=(-2.0f*(f*n)/(f-n));
	  
	  // first column
	  projectionMatrix_[X_INDEX+N_ROWS*X_INDEX]=xScale;inverseProjectionMatrix_[X_INDEX+N_ROWS*X_INDEX]=1/xScale;

	  // second column
	  projectionMatrix_[Y_INDEX+N_ROWS*Y_INDEX]=yScale;inverseProjectionMatrix_[Y_INDEX+N_ROWS*Y_INDEX]=1/yScale;
	  
	  // third column
	  projectionMatrix_[N_ROWS*Z_INDEX+X_INDEX]=-xShift;
	  projectionMatrix_[N_ROWS*Z_INDEX+Y_INDEX]=-yShift;
	  projectionMatrix_[N_ROWS*Z_INDEX+Z_INDEX]=a_;inverseProjectionMatrix_[N_ROWS*Z_INDEX+Z_INDEX]=zShift_/b_;
	  projectionMatrix_[N_ROWS*Z_INDEX+N_SPATIAL_DIMENSIONS]=-1;inverseProjectionMatrix_[N_ROWS*Z_INDEX+N_SPATIAL_DIMENSIONS]=1/b_;
	  
	  // fourth column
	  projectionMatrix_[N_ROWS*N_SPATIAL_DIMENSIONS+X_INDEX]=zShift_*xShift;inverseProjectionMatrix_[N_ROWS*N_SPATIAL_DIMENSIONS+X_INDEX]=-xShift/xScale;
	  projectionMatrix_[N_ROWS*N_SPATIAL_DIMENSIONS+Y_INDEX]=zShift_*yShift;inverseProjectionMatrix_[N_ROWS*N_SPATIAL_DIMENSIONS+Y_INDEX]=-yShift/yScale;
	  projectionMatrix_[N_ROWS*N_SPATIAL_DIMENSIONS+Z_INDEX]=-a_*zShift_+b_;inverseProjectionMatrix_[N_ROWS*N_SPATIAL_DIMENSIONS+Z_INDEX]=a_*zShift_/b_-1;
	  projectionMatrix_[N_ROWS*N_SPATIAL_DIMENSIONS+N_SPATIAL_DIMENSIONS]=zShift_;inverseProjectionMatrix_[N_ROWS*N_SPATIAL_DIMENSIONS+N_SPATIAL_DIMENSIONS]=a_/b_;
	
	}

      glContext_->update_global_uniform_4x4(GLContext::PROJECTION_MATRIX,projectionMatrix_);
      glContext_->update_global_uniform_4x4(GLContext::INVERSE_PROJECTION_MATRIX,inverseProjectionMatrix_);

      glContext_->request_redraw();
    }
dgInt32 dgPolygonSoupDatabaseBuilder::AddConvexFace(dgInt32 count,
    dgInt32* const pool, dgInt32* const facesArray)
{
  dgPolySoupFilterAllocator polyhedra(m_allocator);

  count = polyhedra.AddFilterFace(dgUnsigned32(count), pool);

  dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
  if (edge->m_incidentFace < 0)
  {
    edge = edge->m_twin;
  }

  dgInt32 isconvex = 1;
  dgInt32 facesCount = 0;

  dgInt32 flag = 1;
  while (flag)
  {
    flag = 0;
    if (count >= 3)
    {
      dgEdge* ptr = edge;

      dgBigVector p0(&m_vertexPoints[ptr->m_incidentVertex].m_x);
      do
      {
        dgBigVector p1(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
        dgBigVector e0(p1 - p0);
        dgFloat64 mag2 = e0 % e0;
        if (mag2 < dgFloat32(1.0e-6f))
        {
          count--;
          flag = 1;
          edge = ptr->m_next;
          ptr->m_prev->m_next = ptr->m_next;
          ptr->m_next->m_prev = ptr->m_prev;
          ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
          ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
          break;
        }
        p0 = p1;
        ptr = ptr->m_next;
      } while (ptr != edge);
    }
  }
  if (count >= 3)
  {
    flag = 1;

    while (flag)
    {
      flag = 0;
      if (count >= 3)
      {
        dgEdge* ptr = edge;

        dgBigVector p0(&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
        dgBigVector p1(&m_vertexPoints[ptr->m_incidentVertex].m_x);
        dgBigVector e0(p1 - p0);
        e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
        do
        {
          dgBigVector p2(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
          dgBigVector e1(p2 - p1);

          e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
          dgFloat64 mag2 = e1 % e0;
          if (mag2 > dgFloat32(0.9999f))
          {
            count--;
            flag = 1;
            edge = ptr->m_next;
            ptr->m_prev->m_next = ptr->m_next;
            ptr->m_next->m_prev = ptr->m_prev;
            ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
            ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
            break;
          }

          e0 = e1;
          p1 = p2;
          ptr = ptr->m_next;
        } while (ptr != edge);
      }
    }

    dgBigVector normal(
        polyhedra.FaceNormal(edge, &m_vertexPoints[0].m_x,
            sizeof(dgBigVector)));
    dgFloat64 mag2 = normal % normal;
    if (mag2 < dgFloat32(1.0e-8f))
    {
      return 0;
    }
    normal = normal.Scale(dgRsqrt (mag2));

    if (count >= 3)
    {
      dgEdge* ptr = edge;
      dgBigVector p0(&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
      dgBigVector p1(&m_vertexPoints[ptr->m_incidentVertex].m_x);
      dgBigVector e0(p1 - p0);
      e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
      do
      {
        dgBigVector p2(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
        dgBigVector e1(p2 - p1);

        e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));

        dgBigVector n(e0 * e1);
        dgFloat64 mag2 = n % normal;
        if (mag2 < dgFloat32(1.0e-5f))
        {
          isconvex = 0;
          break;
        }

        e0 = e1;
        p1 = p2;
        ptr = ptr->m_next;
      } while (ptr != edge);
    }
  }

  if (isconvex)
  {
    dgEdge* const first = edge;
    if (count >= 3)
    {
      count = 0;
      dgEdge* ptr = first;
      do
      {
        pool[count] = ptr->m_incidentVertex;
        count++;
        ptr = ptr->m_next;
      } while (ptr != first);
      facesArray[facesCount] = count;
      facesCount = 1;
    }
  }
  else
  {
    dgPolyhedra leftOver(m_allocator);
    dgPolyhedra polyhedra2(m_allocator);
    dgEdge* ptr = edge;
    count = 0;
    do
    {
      pool[count] = ptr->m_incidentVertex;
      count++;
      ptr = ptr->m_next;
    } while (ptr != edge);

    polyhedra2.BeginFace();
    polyhedra2.AddFace(count, pool);
    polyhedra2.EndFace();
    leftOver.BeginFace();
    polyhedra2.ConvexPartition(&m_vertexPoints[0].m_x, sizeof(dgTriplex),
        &leftOver);
    leftOver.EndFace();
    _ASSERTE(leftOver.GetCount() == 0);

    dgInt32 mark = polyhedra2.IncLRU();
    dgInt32 index = 0;
    dgPolyhedra::Iterator iter(polyhedra2);
    for (iter.Begin(); iter; iter++)
    {
      dgEdge* const edge = &(*iter);
      if (edge->m_incidentFace < 0)
      {
        continue;
      }
      if (edge->m_mark == mark)
      {
        continue;
      }

      ptr = edge;
      count = 0;
      do
      {
        ptr->m_mark = mark;
        pool[index] = ptr->m_incidentVertex;
        index++;
        count++;
        ptr = ptr->m_next;
      } while (ptr != edge);

      facesArray[facesCount] = count;
      facesCount++;
    }
  }

  return facesCount;
}
// Perform the work of making a netlist from a single sheet in this design...
void CNetList::MakeNetForSheet( fileCollection &imports, int file_index_id, int &file_name_index, int sheet, CTinyCadDoc *pDesign )
{
  // Get rid of any old data
  m_CurrentNet = 1;
  m_nodes.erase( m_nodes.begin(), m_nodes.end() );
  m_nets.erase( m_nets.begin(), m_nets.end() );

  // Here is some temporary data for this function
  typedef std::map<CString,int> stringCollection;
  stringCollection	Powers;
  stringCollection	Connected;

  // Search for nodes, and build the node tree
  drawingIterator it = pDesign->GetDrawingBegin();
  for (;it != pDesign->GetDrawingEnd(); ++ it ) 
  {
	CDrawingObject *ObjPtr = *it;
	stringCollection::iterator found;
	int hold;
	CDPoint tr;

	switch (ObjPtr->GetType()) 
	{
		case xHierarchicalSymbol:
			{
				CDrawHierarchicalSymbol *pSymbol = static_cast<CDrawHierarchicalSymbol*>(ObjPtr);

				// Try and stop recursion by limiting the number of imports
				if (imports.size() > 100)
				{
					AfxMessageBox( IDS_RECURSION );
					continue;
				}

				// Push back this filename into the list of extra imports
				CImportFile *f = new CImportFile;
				++ file_name_index;
				f->m_file_name_index = file_name_index;
				if (f->Load( pSymbol->GetFilename() ) )
				{
					imports.push_back( f );

					// Now search the symbol for pins to link the other symbols to
					drawingCollection method;
					pSymbol->ExtractSymbol(tr,method);

					drawingIterator it = method.begin();
					while ( it != method.end() ) 
					{
						CDrawingObject *pointer = *it;

						if (pointer->GetType()==xPinEx) 
						{
							CDrawPin *thePin = static_cast<CDrawPin*>(pointer);

							// This in effect labels the node with the new node name...
							CNetListNode n( file_name_index, sheet, thePin, thePin->GetActivePoint(pSymbol) );
							n.setLabel( thePin->GetPinName() );
							n.m_reference = pSymbol->GetRefSheet(m_prefix_references,m_prefix_import,file_index_id,sheet);
							n.m_pin = thePin->GetNumber();
							n.m_pMethod = pSymbol;
							Add(n);
						}

						++ it;
					}
				}
				else
				{
					delete f;
				}
			}
			break;
		case xMethodEx3:
			#define thePin ((CDrawPin*)pointer)
			#define theMethod ((CDrawMethod*)ObjPtr)
			{
				drawingCollection method;
				((CDrawMethod *)ObjPtr)->ExtractSymbol(tr,method);

				drawingIterator it = method.begin();
				while ( it != method.end() ) 
				{
					CDrawingObject *pointer = *it;

					if (pointer->GetType()==xPinEx && !(thePin->IsPower()) ) 
					{
						CNetListNode n( file_index_id, sheet, thePin,thePin->GetActivePoint(theMethod));
						n.m_reference = theMethod->GetRefSheet(m_prefix_references,m_prefix_import,file_index_id,sheet);
						n.m_pin = thePin->GetNumber();
						n.m_pMethod = theMethod;
						Add(n);
					}

					++ it;
				}

				// Has this symbol had it's power connected?
				if (Connected.find(theMethod->GetRefSheet(m_prefix_references,m_prefix_import,file_index_id,sheet)) == Connected.end()) 
				{
					Connected[ theMethod->GetRefSheet(m_prefix_references,m_prefix_import,file_index_id,sheet) ] = TRUE;

					drawingCollection method;
					((CDrawMethod *)ObjPtr)->ExtractSymbol(tr,method);
					drawingIterator it = method.begin();
					while (it!=method.end()) 
					{
						CDrawingObject *pointer = *it;

						if (pointer->GetType()==xPinEx && thePin->IsPower()) 
						{
							CNetListNode n(file_index_id, sheet, thePin,thePin->GetActivePoint(theMethod) );
							n.m_reference = theMethod->GetRefSheet(m_prefix_references,m_prefix_import,file_index_id,sheet);
							n.m_pin = thePin->GetNumber();
							n.m_pMethod = theMethod;


							// Look up the netlist this power belongs to
							found = Powers.find( thePin->GetPinName() );
							if (found != Powers.end())
								n.m_NetList = (*found).second;
							hold = Add(n);
							if (found == Powers.end())
								Powers[thePin->GetPinName()] = hold;
						}

						++ it;
					}
				}
			}			
			break;
		case xNoConnect:
			Add(CNetListNode(file_index_id, sheet, ObjPtr,ObjPtr->m_point_a));			
			break;
		case xJunction:
			Add(CNetListNode(file_index_id, sheet, ObjPtr,ObjPtr->m_point_a));
			break;			
		case xPower:
			{
			CNetListNode n(file_index_id, sheet, ObjPtr,ObjPtr->m_point_a);
			n.setLabel( ((CDrawPower *)ObjPtr)->GetValue() );

			// Does this power item exist?
			found = Powers.find(((CDrawPower *)ObjPtr)->GetValue());
			if (found != Powers.end())
				n.m_NetList = (*found).second;
			hold = Add(n);
			if (found == Powers.end())
				Powers[((CDrawPower *)ObjPtr)->GetValue()] = hold;
			}
			break;
		case xWire:
			{
				CNetListNode n(file_index_id, sheet, ObjPtr,ObjPtr->m_point_a);
				hold = Add(n);
			}
			{
				CNetListNode n(file_index_id, sheet, ObjPtr,ObjPtr->m_point_b);
				n.m_NetList = hold;
				Add(n);
			}
			break;
	}
  }

 

  // Search for junctions and connect together
  it = pDesign->GetDrawingBegin();
  while (it != pDesign->GetDrawingEnd()) 
  {
	CDrawingObject *ObjPtr = *it;

	// Search for junctions
	if (ObjPtr->GetType() == xJunction) 
	{
		// Find out which netlist was assigned to this point
		CDPoint a = ObjPtr->m_point_a;
		int NetNumber = m_nodes[ a ];

		// Look for wires which cross this junction
		drawingIterator search_it = pDesign->GetDrawingBegin();
		while (search_it != pDesign->GetDrawingEnd()) 
		{
			CDrawingObject *search = *search_it;

			// Find the wires
			// If the wire has an end at this junction then it is already connected
			if (search->GetType()==xWire 
			 && search->m_point_a!=a && search->m_point_b!=a)
			{
				// Is this point on this wire?
				CLineUtils l( search->m_point_a, search->m_point_b );
				double distance_along_a;

				if (l.IsPointOnLine( a, distance_along_a ))
				{
					CNetListNode n(file_index_id, sheet, NULL,search->m_point_a);
					n.m_NetList = NetNumber;
					NetNumber = Add(n);
				}
			}

			++ search_it;
		}
	}
	
	++ it;
  }

  // Search for labels and connect to their respective lines
  stringCollection labels;
  it = pDesign->GetDrawingBegin();
  while (it != pDesign->GetDrawingEnd()) 
  {
	CDrawingObject *ObjPtr = *it;

	// Search for junctions
	if (ObjPtr->GetType() == xLabelEx2) 
	{
		CDPoint a = static_cast<CDrawLabel*>(ObjPtr)->GetLabelPoint();

		// Search for a wire this label is connect to
		// Only atempt to connect to a single wire
		drawingIterator search_it = pDesign->GetDrawingBegin();
		while (search_it != pDesign->GetDrawingEnd()) 
		{
			CDrawingObject *search = *search_it;
			if (search->GetType()==xWire && search->IsInside(a.x,a.x,a.y,a.y)) 
			{
				a = search->m_point_a;
				break;
			}
			
			++ search_it;
		}

		// Look up this label
		CNetListNode n(file_index_id, sheet, ObjPtr,a);
		n.setLabel(  ((CDrawLabel *)ObjPtr)->GetValue() );

		// Has this label already been assigned a netlist?
		stringCollection::iterator found = labels.find(((CDrawLabel *)ObjPtr)->GetValue());
		if (found!=labels.end()) 
		{
			n.m_NetList = (*found).second;
			n.setLabel( _T("") );
		}

		int hold = Add(n);

		// If there was no netlist write it back...
		if (found == labels.end())
			labels[ ((CDrawLabel *)ObjPtr)->GetValue()] = hold;
	}
	
	++ it;
  }  

  // Our work with the nodes map is complete, so we can discard
  // it now...
  m_nodes.erase( m_nodes.begin(), m_nodes.end() );

}
Exemple #23
0
bool UnitSquare::intersect( Ray3D& ray, const Matrix4x4& worldToModel,
        const Matrix4x4& modelToWorld ) {
    // TODO: implement intersection code for UnitSquare, which is
    // defined on the xy-plane, with vertices (0.5, 0.5, 0), 
    // (-0.5, 0.5, 0), (-0.5, -0.5, 0), (0.5, -0.5, 0), and normal
    // (0, 0, 1).
    //
    // Your goal here is to fill ray.intersection with correct values
    // should an intersection occur.  This includes intersection.point, 
    // intersection.normal, intersection.none, intersection.t_value.   
    //
    // HINT: Remember to first transform the ray into object space  
    // to simplify the intersection test.

    Vector3D rayDir = worldToModel * ray.dir;
    Point3D rayOri = worldToModel * ray.origin;

    double lamda = std::numeric_limits<double>::infinity();;

    if (rayDir[2] == 0) {
        if (rayOri[2] == 0) {
            double lamda1 = (-0.5 - rayOri[0]) / rayDir[0];
            double lamda2 = (0.5 - rayOri[0]) / rayDir[0];
            double lamda3 = (-0.5 - rayOri[1]) / rayDir[1];
            double lamda4 = (0.5 - rayOri[1]) / rayDir[1];

            Point3D inter1 = rayOri + lamda1 * rayDir;
            Point3D inter2 = rayOri + lamda2 * rayDir;
            Point3D inter3 = rayOri + lamda3 * rayDir;
            Point3D inter4 = rayOri + lamda4 * rayDir;

            if (inter1[1] <= 0.5 && inter1[1] >= -0.5) {
                lamda = std::min(lamda1, lamda);
            } else if (inter2[1] <= 0.5 && inter2[1] >= -0.5) {
                lamda = std::min(lamda2, lamda);
            } else if (inter3[0] <= 0.5 && inter3[0] >= -0.5) {
                lamda = std::min(lamda3, lamda);
            } else {
                lamda = std::min(lamda4, lamda);
            }
        } else {
            return false;
        }
    } else {
        lamda = -rayOri[2] / rayDir[2];
    }

    if (!ray.intersection.none && lamda > ray.intersection.t_value) {
        return false;
    }

    Point3D intersection = rayOri + lamda * rayDir;
    if (intersection[0] > 0.5 || intersection[0] < -0.5 || intersection[1] > 0.5 || intersection[1] < -0.5) {
        return false;
    } else {
        ray.intersection.point = modelToWorld * intersection;
        ray.intersection.none = false;
        ray.intersection.t_value = lamda;

        Vector3D n(0, 0, 1);
        ray.intersection.normal = modelToWorld * n;

        return true;
    }
}
Exemple #24
0
//------------------------------------------------------------------------------
void PD_LPS::applySurfaceCorrection(double strain)
{
    arma::vec3 strainFactor;
    arma::mat gd = arma::zeros(m_particles.nParticles(), m_dim); // Dilation correction
    arma::mat gb = arma::zeros(m_particles.nParticles(), m_dim); // Bond correction

    //--------------------------------------------------------------------------
    // Apllying correction to the dilation term
    //--------------------------------------------------------------------------
    // Stretching all particle in the x, y and z-direction
    strainFactor(0) = strain;
    strainFactor(1) = 0;
    strainFactor(2) = 0;

    for(int a=0; a<m_dim; a++)
    {
        if(a == 1)
            strainFactor.swap_rows(0,1);
        else if(a == 2)
            strainFactor.swap_rows(1,2);

#ifdef USE_OPENMP
# pragma omp parallel for
#endif
        // Applying uniaxial stretch
        for(int i=0; i<m_particles.nParticles(); i++)
        {
            pair<int, int> idCol(i, i);
            const int col_i = idCol.second;

            for(int d=0; d<m_dim; d++)
            {
                m_r(d, col_i) = (1 + strainFactor(d))*m_r(d, col_i);
            }
        }

        double W_d = strain;
#ifdef USE_OPENMP
# pragma omp parallel for
#endif
        // Calculating the elastic energy density
        for(int i=0; i<m_particles.nParticles(); i++)
        {
            pair<int, int> idCol(i, i);
            const int col_i = idCol.second;
            const double theta_i = computeDilation(idCol);
            const double factor =  W_d/theta_i;
            gd(col_i, a) = factor;
        }

#ifdef USE_OPENMP
# pragma omp parallel for
#endif
        // Resetting the positions
        for(int i=0; i<m_particles.nParticles(); i++)
        {
            pair<int, int> idCol(i, i);
            int col_i = idCol.second;

            for(int d=0; d<m_dim; d++)
            {
//                m_r(d, col_i) = m_r(d, col_i)/(1 + strainFactor(d));
                m_r(d, col_i) = m_r0(d, col_i);
            }
        }
    }

    //--------------------------------------------------------------------------
    // Applying correction to the bond term
    //--------------------------------------------------------------------------
    // Performing a simple shear of all particle in the x, y and z-direction
    arma::ivec3 axis;
    strainFactor(0) = strain;
    strainFactor(1) = 0.*strain;
    strainFactor(2) = 0;
    axis(0) = 1;
    axis(1) = 0;
    axis(2) = 0;

    for(int a=0; a<m_dim; a++)
    {
        if(a == 1)
        {
            strainFactor.swap_rows(1,2);
            strainFactor.swap_rows(0,1);
            axis(0) = 2;
            axis(1) = 0;
            axis(2) = 1;
        }
        else if(a == 2)
        {
            strainFactor.swap_rows(2,0);
            strainFactor.swap_rows(1,2);
            axis(0) = 2;
            axis(1) = 0;
            axis(2) = 0;
        }
//#ifdef USE_OPENMP
//# pragma omp parallel for
//#endif
        // Applying uniaxial stretch
        for(int i=0; i<m_particles.nParticles(); i++)
        {
            pair<int, int> idCol(i, i);
            int col_i = idCol.second;

            for(int d=0; d<m_dim; d++)
            {
                double shear = strainFactor(d)*m_r(axis(d), col_i);
                m_r(d, col_i) = m_r(d, col_i) + shear;
            }
        }

        double W_s = 0.5*m_mu*strain*strain;
//#ifdef USE_OPENMP
        //# pragma omp parallel for
        //#endif
        // Calculating the elastic energy density
        for(int i=0; i<m_particles.nParticles(); i++)
        {
            pair<int, int> idCol(i, i);
            const int col_i = idCol.second;
            const double bond_i = calculatePotentialEnergyDensity(idCol);
            const double factor =  W_s/bond_i;
            gb(col_i, a) = factor;
        }

        //#ifdef USE_OPENMP
//# pragma omp parallel for
//#endif
        // Resetting the positions
        for(int i=0; i<m_particles.nParticles(); i++)
        {
            pair<int, int> idCol(i, i);
            int col_i = idCol.second;

            for(int d=0; d<m_dim; d++)
            {
//                m_r(d, col_i) = m_r(d, col_i) - strainFactor(d)*m_r(axis(d), col_i);
                m_r(d, col_i) = m_r0(d, col_i);
            }
        }
    }

    //--------------------------------------------------------------------------
    // Calculating the scaling
    //--------------------------------------------------------------------------
//#ifdef USE_OPENMP
//# pragma omp parallel for
//#endif
    for(int i=0; i<m_particles.nParticles(); i++)
    {
        pair<int, int> idCol(i, i);
        int pId = idCol.first;
        int col_i = idCol.second;

        vector<pair<int, vector<double>>> & PDconnections = m_particles.pdConnections(pId);

        for(auto &con:PDconnections)
        {
            int id_j = con.first;
            int col_j = m_pIds[id_j];

            double dr0Len = con.second[m_iDr0];
            arma::vec3 n = (m_r.col(col_i) - m_r.col(col_j))/dr0Len;

            arma::vec3 gd_mean;
            arma::vec3 gb_mean;
            double Gd = 0;
            double Gb = 0;
            for(int d=0; d<m_dim; d++)
            {
                const double gd_i = gd(col_i, d);
                const double gd_j = gd(col_j, d);
                const double gb_i = gb(col_i, d);
                const double gb_j = gb(col_j, d);
                gd_mean(d) = 0.5*(gd_i + gd_j);
                gb_mean(d) = 0.5*(gb_i + gb_j);
                Gb += pow(n(d)/gb_mean(d), 2);
                Gd += pow(n(d)/gd_mean(d), 2);
            }

            Gd = pow(Gd, -0.5);
            Gb = pow(Gb, -0.5);
            con.second[m_iForceScalingDilation] *= Gd;
            con.second[m_iForceScalingBond] *= Gb;
        }
    }
}
bool ChainParser::parseChain(XmlRpc::XmlRpcValue& chain_description, Tree tree, urdf::Model& robot_model,
                               std::map<std::string, unsigned int>& joint_name_to_index,
                               std::vector<std::string>& index_to_joint_name,
                               std::vector<double>& q_min, std::vector<double>& q_max) {
    ros::NodeHandle n("~");
    std::string ns = n.getNamespace();

    std::cout << chain_description << std::endl;

    if (chain_description.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
        ROS_ERROR("Chain description should be a struct containing 'root' and 'tip'. (namespace: %s)", n.getNamespace().c_str());
        return false;
    }

    XmlRpc::XmlRpcValue& v_root_link = chain_description["root"];
    if (v_root_link.getType() != XmlRpc::XmlRpcValue::TypeString) {
        ROS_ERROR("Chain description for does not contain 'root'. (namespace: %s)", n.getNamespace().c_str());
        return false;
    }
    std::string root_link_name = (std::string)v_root_link;

    XmlRpc::XmlRpcValue& v_tip_link = chain_description["tip"];
    if (v_tip_link.getType() != XmlRpc::XmlRpcValue::TypeString) {
        ROS_ERROR("Chain description for does not contain 'tip'. (namespace: %s)", n.getNamespace().c_str());
        return false;
    }
    std::string tip_link_name = (std::string)v_tip_link;

    Chain* chain = new Chain();

    ROS_INFO("Looking for chain from %s to %s", root_link_name.c_str(), tip_link_name.c_str());

    if (!tree.kdl_tree_.getChain(root_link_name, tip_link_name, chain->kdl_chain_)) {
//    if (!tree.kdl_tree_.getChain("base", tip_link_name, chain->kdl_chain_)) {
        ROS_FATAL("Could not initialize chain object");
        return false;
    }

    for(unsigned int i = 0; i < chain->kdl_chain_.getNrOfSegments(); ++i) {
        const KDL::Segment& segment = chain->kdl_chain_.getSegment(i);
        const KDL::Joint& joint = segment.getJoint();

        if (joint.getType() != KDL::Joint::None)
        {
            //cout << "Segment: " << segment.getName() << endl;
            //cout << "Joint:   " << joint.getName() << endl;

            unsigned int full_joint_index = 0;

            std::map<std::string, unsigned int>::iterator it_joint = joint_name_to_index.find(joint.getName());

            if (it_joint == joint_name_to_index.end()) {
                // joint name is not yet in map, so give it a new fresh index and add it to the map
                full_joint_index = joint_name_to_index.size();

                //cout << "    new joint, gets index " << full_joint_index << endl;
                //cout << "    type: " << joint.getTypeName() << endl;

                joint_name_to_index[joint.getName()] = full_joint_index;
                index_to_joint_name.push_back(joint.getName());

                //cout << "    Lower limit: " << robot_model.getJoint(joint.getName())->limits->lower << endl;
                //cout << "    Upper limit: " << robot_model.getJoint(joint.getName())->limits->upper << endl;

                // determine joint limits from URDF
                q_min.push_back(robot_model.getJoint(joint.getName())->limits->lower);
                q_max.push_back(robot_model.getJoint(joint.getName())->limits->upper);

            } else {
                // joint name already in the map, so look-up its index
                full_joint_index = it_joint->second;

                //cout << "    existing joint, has index: " << full_joint_index << endl;
            }

        }

    }

    return true;
}
dgFloat32 dgCollisionInstance::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
	if (!preFilter || preFilter(body, this, userData)) {
		switch(m_scaleType)
		{
			case m_unit:
			{
				dgFloat32 t = m_childShape->RayCast (localP0, localP1, maxT, contactOut, body, userData, preFilter);
				if (t <= maxT) {
					if (!(m_childShape->IsType(dgCollision::dgCollisionMesh_RTTI) || m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) {
						contactOut.m_shapeId0 = GetUserDataID();
						contactOut.m_shapeId1 = GetUserDataID();
					}
					if (!m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI)) {
						contactOut.m_collision0 = this;
						contactOut.m_collision1 = this;
					}
				}
				return t;
			}

			case m_uniform:
			{
				dgVector p0 (localP0.CompProduct4(m_invScale));
				dgVector p1 (localP1.CompProduct4(m_invScale));
				dgFloat32 t = m_childShape->RayCast (p0, p1, maxT, contactOut, body, userData, preFilter);
				if (t <= maxT) {
					if (!(m_childShape->IsType(dgCollision::dgCollisionMesh_RTTI) || m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) {
						contactOut.m_shapeId0 = GetUserDataID();
						contactOut.m_shapeId1 = GetUserDataID();
					}
					if (!m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI)) {
						contactOut.m_collision0 = this;
						contactOut.m_collision1 = this;
					}
				}
				return t;
			}

			case m_nonUniform:
			{
				dgVector p0 (localP0.CompProduct4(m_invScale));
				dgVector p1 (localP1.CompProduct4(m_invScale));
				dgFloat32 t = m_childShape->RayCast (p0, p1, maxT, contactOut, body, userData, preFilter);
				if (t <= maxT) {
					if (!(m_childShape->IsType(dgCollision::dgCollisionMesh_RTTI) || m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) {
						contactOut.m_shapeId0 = GetUserDataID();
						contactOut.m_shapeId1 = GetUserDataID();
						dgVector n (m_invScale.CompProduct4 (contactOut.m_normal));
						contactOut.m_normal = n.CompProduct4(n.InvMagSqrt());
					}
					if (!m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI)) {
						contactOut.m_collision0 = this;
						contactOut.m_collision1 = this;
					}
				}
				return t;
			}

			case m_global:
			default:
			{
				dgVector p0 (m_aligmentMatrix.UntransformVector (localP0.CompProduct4(m_invScale)));
				dgVector p1 (m_aligmentMatrix.UntransformVector (localP1.CompProduct4(m_invScale)));
				dgFloat32 t = m_childShape->RayCast (p0, p1, maxT, contactOut, body, userData, preFilter);
				if (t <= maxT) {
					if (!(m_childShape->IsType(dgCollision::dgCollisionMesh_RTTI) || m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) {
						contactOut.m_shapeId0 = GetUserDataID();
						contactOut.m_shapeId1 = GetUserDataID();
						dgVector n (m_aligmentMatrix.RotateVector(m_invScale.CompProduct4 (contactOut.m_normal)));
						contactOut.m_normal = n.CompProduct4(n.InvMagSqrt());
					}
					if (!(m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) {
						contactOut.m_collision0 = this;
						contactOut.m_collision1 = this;
					}
				}
				return t;
			}
		}
	}
	return dgFloat32 (1.2f);

}
Exemple #27
0
/*---------------------------------------------------------------------------*/
int
tcpdump_format(u8_t *packet, u16_t packetlen,
	       char *buf, u16_t buflen)
{
  char flags[8];
  if(IPBUF->proto == UIP_PROTO_ICMP) {
    if(ICMPBUF->type == ICMP_ECHO) {
      return s(" ping",
	     n(IPBUF->destipaddr[3], d(
	     n(IPBUF->destipaddr[2], d(
	     n(IPBUF->destipaddr[1], d(
	     n(IPBUF->destipaddr[0],
             s(" ",
	     n(IPBUF->srcipaddr[3], d(
	     n(IPBUF->srcipaddr[2], d(
	     n(IPBUF->srcipaddr[1], d(
             n(IPBUF->srcipaddr[0],
	     buf)))))))))))))))) - buf;
	     
      /*      return sprintf(buf, "%d.%d.%d.%d %d.%d.%d.%d ping",
		     IPBUF->srcipaddr[0], IPBUF->srcipaddr[1],
		     IPBUF->srcipaddr[2], IPBUF->srcipaddr[3],
		     IPBUF->destipaddr[0], IPBUF->destipaddr[1],
		     IPBUF->destipaddr[2], IPBUF->destipaddr[3]);*/
    } else if(ICMPBUF->type == ICMP_ECHO_REPLY) {
      return s(" pong",
	     n(IPBUF->destipaddr[3], d(
	     n(IPBUF->destipaddr[2], d(
	     n(IPBUF->destipaddr[1], d(
	     n(IPBUF->destipaddr[0],
             s(" ",
	     n(IPBUF->srcipaddr[3], d(
	     n(IPBUF->srcipaddr[2], d(
	     n(IPBUF->srcipaddr[1], d(
             n(IPBUF->srcipaddr[0],
	     buf)))))))))))))))) - buf;
      /*      return sprintf(buf, "%d.%d.%d.%d %d.%d.%d.%d pong",
		     IPBUF->srcipaddr[0], IPBUF->srcipaddr[1],
		     IPBUF->srcipaddr[2], IPBUF->srcipaddr[3],
		     IPBUF->destipaddr[0], IPBUF->destipaddr[1],
		     IPBUF->destipaddr[2], IPBUF->destipaddr[3]);*/
    }
  } else if(IPBUF->proto == UIP_PROTO_UDP) {
      return s(" UDP",
	     n(uip_htons(UDPBUF->destport), d(
	     n(IPBUF->destipaddr[3], d(
	     n(IPBUF->destipaddr[2], d(
	     n(IPBUF->destipaddr[1], d(
	     n(IPBUF->destipaddr[0],
             s(" ",
	     n(uip_htons(UDPBUF->srcport), d(
	     n(IPBUF->srcipaddr[3], d(
	     n(IPBUF->srcipaddr[2], d(
	     n(IPBUF->srcipaddr[1], d(
             n(IPBUF->srcipaddr[0],
	     buf)))))))))))))))))))) - buf;
      /*    return sprintf(buf, "%d.%d.%d.%d.%d %d.%d.%d.%d.%d UDP",
		   IPBUF->srcipaddr[0], IPBUF->srcipaddr[1],
		   IPBUF->srcipaddr[2], IPBUF->srcipaddr[3],
		   uip_htons(UDPBUF->srcport),
		   IPBUF->destipaddr[0], IPBUF->destipaddr[1],
		   IPBUF->destipaddr[2], IPBUF->destipaddr[3],
		   uip_htons(UDPBUF->destport));*/
  } else if(IPBUF->proto == UIP_PROTO_TCP) {
    tcpflags(TCPBUF->flags, flags);
      return s(flags,
             s(" ",
	     n(uip_htons(TCPBUF->destport), d(
	     n(IPBUF->destipaddr[3], d(
	     n(IPBUF->destipaddr[2], d(
	     n(IPBUF->destipaddr[1], d(
	     n(IPBUF->destipaddr[0],
             s(" ",
	     n(uip_htons(TCPBUF->srcport), d(
	     n(IPBUF->srcipaddr[3], d(
	     n(IPBUF->srcipaddr[2], d(
	     n(IPBUF->srcipaddr[1], d(
             n(IPBUF->srcipaddr[0],
	     buf))))))))))))))))))))) - buf;
    /*    return sprintf(buf, "%d.%d.%d.%d.%d %d.%d.%d.%d.%d %s",
		   IPBUF->srcipaddr[0], IPBUF->srcipaddr[1],
		   IPBUF->srcipaddr[2], IPBUF->srcipaddr[3],
		   uip_htons(TCPBUF->srcport),
		   IPBUF->destipaddr[0], IPBUF->destipaddr[1],
		   IPBUF->destipaddr[2], IPBUF->destipaddr[3],
		   uip_htons(TCPBUF->destport),
		   flags);  */
  } else {
    strcpy(buf, "Unrecognized protocol");
  }

  return 0;
}
void Bonsai::buildAll()
{
	// generate a unique random seed for the whole thing from the name
	DBG::info("Generate planet '%s'\n", m_name );
	int master_seed = 1;
	for (const char *ch=m_name; *ch; ch++)
	{
		master_seed *= (*ch);
	}

	// generate random seeds for each step .. this keeps them
	// somewhat resistant to changes in the algoritms 
	int pally_seed, param_seed, terrain_seed;
	srand( master_seed );
	pally_seed = rand();
	terrain_seed = rand();
	param_seed = rand();
		
	// Make the palette
	m_pally.generate( pally_seed );

	// synthesize the height data
	m_hiteData = new float [ HITE_RES * HITE_RES ];
	m_terrainColor = new GLubyte [ HITE_RES * HITE_RES * 3 ];
	m_terrainNorm = new GLubyte [ HITE_RES * HITE_RES * 3 ];

	// Set up params
	srand( param_seed );
	m_params.offs = randUniform( 0.0, 1000.0 );

	m_params.base = (int)randUniform( 0, NUM_BASE );	
	m_params.base_distAmt = randUniform( 0.0, 0.3 );
	m_params.base_scale = randUniform( 0.3, 1.0 );	

	m_params.baseVeg = (int)randUniform( 0, NUM_BASEVEG );	

	m_params.baseVeg_hite_repeats = (int)randUniform( 1, 5 );		
	m_params.baseVeg_hite_repeats *= m_params.baseVeg_hite_repeats;	

	m_params.patchVeg = (int)randUniform( 0, NUM_PATCHVEG );	
	m_params.patchVeg_scale = randUniform( 1.0, 20.0 );
	m_params.patchVeg_nscale = randUniform( 0.01, 8.0 );
	m_params.patchVeg_nscale *= m_params.patchVeg_nscale;
	m_params.patchMixMineral = (randUniform() > 0.5)?1:0;
	m_params.patchVeg_thresh = randUniform( 0.55, 0.85 );	

	// Decoration
	m_params.deco = (int)randUniform( 0, NUM_DECORATION );	
	m_params.decoLavaScale = randUniform( 1.0, 10.0 );
	m_params.decoLavaWidth = randUniform( 0.01, 0.1 );

	// =============================================

	// build textures
	Luddite::HTexture htexColor;
	Luddite::HTexture htexNorms;	
	

	// check if there is cached image data available		
	htexColor = _checkCachedColorImage( "DIF0", m_terrainColor, HITE_RES);
	if ( (!htexColor.isNull() ) && (_checkCachedHeight( "HITE", m_hiteData, HITE_RES ) ))
	{
		DBG::info( "Land '%s' read cached color and height.\n", m_name );
	}
	else
	{
		// Synthesize
		srand( terrain_seed );

		// step for tuning/debugging quicker
		// step=1 for final quality
		int step = SYNTH_STEP;
		for (int j=0; j < HITE_RES; j += step)
		{
			// status
			progress( "Generating", j, HITE_RES );			

			for (int i=0; i < HITE_RES; i += step )
			{
				float ii = (((float)i / (float)HITE_RES) * 2.0) - 1.0;
				float jj = (((float)j / (float)HITE_RES) * 2.0) - 1.0;

				// synthesize a texel
				size_t ndx = (j*HITE_RES)+i;
				synthesize( ndx, ii, jj );

				for (int sj = 0; sj < step; sj++)
				{
					for(int si=0; si < step; si++)
					{			
						if (si || sj)
						{
							size_t ndx2 = ((j+sj)*HITE_RES)+(i+si);

							m_terrainColor[ndx2 * 3 + 0] = m_terrainColor[ndx * 3 + 0];
							m_terrainColor[ndx2 * 3 + 1] = m_terrainColor[ndx * 3 + 1];
							m_terrainColor[ndx2 * 3 + 2] = m_terrainColor[ndx * 3 + 2];

							m_hiteData[ndx2] = m_hiteData[ndx];

						}
					}
				}
			}
		}

		// Calculate (fake) ambient occlusion
		for (int j=0; j < HITE_RES; j += 1)
		{
			progress( "AmbOccl", j, HITE_RES );

			for (int i=0; i < HITE_RES; i += 1 )
			{
				float ambOccl = 0.0f;
				int c = 0;
				size_t ndx = (j*HITE_RES)+i;

				// TODO: gaussian weight
				for (int ii=-8; ii < 8; ii += 2)
				{
					for (int jj=-8; jj < 8; jj += 2)
					{
						int i2 = i+ii;
						int j2 = j+jj;
						c += 1;
						
						float occl = 0.0;
						if ((i2 >=0) && (i2 < HITE_RES)  &&
							(j2 >=0) && (j2 < HITE_RES) )
						{
							size_t ndx2 = (j2*HITE_RES)+i2;
							occl = m_hiteData[ndx2] - m_hiteData[ndx];							
							occl = fstep( 0.0, 0.015, occl );														
							
							ambOccl += occl;
						}						
					}
				}

				// avg
				ambOccl /= c;
				ambOccl = clamp( 1.0-ambOccl, 0.0, 1.0 );

				// darken color image (dbg replace)
#if 0
				m_terrainColor[ndx * 3 + 0] = (int)(ambOccl * 255);
				m_terrainColor[ndx * 3 + 1] = (int)(ambOccl * 255);
				m_terrainColor[ndx * 3 + 2] = (int)(ambOccl * 255);
#else
				m_terrainColor[ndx * 3 + 0] *= ambOccl;
				m_terrainColor[ndx * 3 + 1] *= ambOccl;
				m_terrainColor[ndx * 3 + 2] *= ambOccl;
#endif

			}
		}
				

		// Cache the height & color data
		SDL_WM_SetCaption( "[Cache Height/Color] LD19 Jovoc - Discovery", NULL );

		_cacheColorImage( "DIF0", m_terrainColor, HITE_RES );
		_cacheHeight( "HITE", m_hiteData, HITE_RES );
	}

	// check if there is cached normal data available
	htexNorms = _checkCachedColorImage( "NRM", m_terrainNorm, HITE_RES);
	if ( !htexNorms.isNull() )
	{
		DBG::info( "Land '%s' read cached normals.\n", m_name );
	}
	else
	{
		// Shitty calc normals -- ideally this should use the
		// height samples to not soften it but for now do it this
		// way cause it's easier/faster
		for (int j=0; j < HITE_RES-1; j++)
		{
			progress( "Calc Normals", j, HITE_RES );

			for (int i=0; i < HITE_RES-1; i++ )
			{
				PVRTVec3 nrm;
				size_t ndx = (j*HITE_RES)+i;
				float a = m_hiteData[ndx];
				float b = m_hiteData[ (j*HITE_RES)+(i+1) ];
				float c = m_hiteData[ ((j+1)*HITE_RES)+i ];

				//float scl = 5.4;
				float scl = 50.0f;
				float dy1 = (b - a) * scl;
				float dy2 = (c - a) * scl;				

				PVRTVec3 n( dy1, dy2, 1.0 - sqrt( dy1*dy1 + dy2*dy2 ) );
				n.normalize();

				m_terrainNorm[ndx * 3 + 0] = (int)((n.x * 128.0) + 128.0);
				m_terrainNorm[ndx * 3 + 1] = (int)((n.y * 128.0) + 128.0);
				m_terrainNorm[ndx * 3 + 2] = (int)((n.z * 128.0) + 128.0);
			}
		}

		// Cache the normal map
		SDL_WM_SetCaption( "[Cache Normals] LD19 Jovoc - Discovery", NULL );
		_cacheColorImage( "NRM", m_terrainNorm, HITE_RES );
	}

	SDL_WM_SetCaption( "LD19 Jovoc - Discovery [Press TAB to ungrab mouse]", NULL );

	// Build texture
	static int treeId = 0;
	
	Luddite::TextureDB *texDB = Luddite::TextureDB::singletonPtr();
	if (htexColor.isNull())
	{
		char s[20];
		sprintf( s, "treeland%d", treeId++);
		htexColor = texDB->buildTextureFromData( s, m_terrainColor, 1024, 1024 );
	}

	if (htexNorms.isNull())
	{
		char s[20];
		sprintf( s, "treenorms%d", treeId++);
		htexNorms = texDB->buildTextureFromData( s, m_terrainNorm, 1024, 1024 );
	}

	// Make a land part
	m_treeLand = new TreeLand( m_hiteData, htexColor, htexNorms );
	m_treeLand->build();
}
UBool
TimeZone::parseCustomID(const UnicodeString& id, int32_t& sign,
                        int32_t& hour, int32_t& min, int32_t& sec) {
    static const int32_t         kParseFailed = -99999;

    NumberFormat* numberFormat = 0;
    UnicodeString idUppercase = id;
    idUppercase.toUpper();

    if (id.length() > GMT_ID_LENGTH &&
        idUppercase.startsWith(GMT_ID))
    {
        ParsePosition pos(GMT_ID_LENGTH);
        sign = 1;
        hour = 0;
        min = 0;
        sec = 0;

        if (id[pos.getIndex()] == MINUS /*'-'*/) {
            sign = -1;
        } else if (id[pos.getIndex()] != PLUS /*'+'*/) {
            return FALSE;
        }
        pos.setIndex(pos.getIndex() + 1);

        UErrorCode success = U_ZERO_ERROR;
        numberFormat = NumberFormat::createInstance(success);
        if(U_FAILURE(success)){
            return FALSE;
        }
        numberFormat->setParseIntegerOnly(TRUE);

        // Look for either hh:mm, hhmm, or hh
        int32_t start = pos.getIndex();
        Formattable n(kParseFailed);
        numberFormat->parse(id, n, pos);
        if (pos.getIndex() == start) {
            delete numberFormat;
            return FALSE;
        }
        hour = n.getLong();

        if (pos.getIndex() < id.length()) {
            if (pos.getIndex() - start > 2
                || id[pos.getIndex()] != COLON) {
                delete numberFormat;
                return FALSE;
            }
            // hh:mm
            pos.setIndex(pos.getIndex() + 1);
            int32_t oldPos = pos.getIndex();
            n.setLong(kParseFailed);
            numberFormat->parse(id, n, pos);
            if ((pos.getIndex() - oldPos) != 2) {
                // must be 2 digits
                delete numberFormat;
                return FALSE;
            }
            min = n.getLong();
            if (pos.getIndex() < id.length()) {
                if (id[pos.getIndex()] != COLON) {
                    delete numberFormat;
                    return FALSE;
                }
                // [:ss]
                pos.setIndex(pos.getIndex() + 1);
                oldPos = pos.getIndex();
                n.setLong(kParseFailed);
                numberFormat->parse(id, n, pos);
                if (pos.getIndex() != id.length()
                        || (pos.getIndex() - oldPos) != 2) {
                    delete numberFormat;
                    return FALSE;
                }
                sec = n.getLong();
            }
        } else {
            // Supported formats are below -
            //
            // HHmmss
            // Hmmss
            // HHmm
            // Hmm
            // HH
            // H

            int32_t length = pos.getIndex() - start;
            if (length <= 0 || 6 < length) {
                // invalid length
                delete numberFormat;
                return FALSE;
            }
            switch (length) {
                case 1:
                case 2:
                    // already set to hour
                    break;
                case 3:
                case 4:
                    min = hour % 100;
                    hour /= 100;
                    break;
                case 5:
                case 6:
                    sec = hour % 100;
                    min = (hour/100) % 100;
                    hour /= 10000;
                    break;
            }
        }

        delete numberFormat;

        if (hour > kMAX_CUSTOM_HOUR || min > kMAX_CUSTOM_MIN || sec > kMAX_CUSTOM_SEC) {
            return FALSE;
        }
        return TRUE;
    }
    return FALSE;
}
PdfContentsGraph::PdfContentsGraph( PdfContentsTokenizer & contentsTokenizer )
    : m_graph()
{
    EPdfContentsType t;
    const char * kwText;
    PdfVariant var;
    bool readToken;

    // Keep a count of the number of tokens read so we can report errors
    // more usefully.
    int tokenNumber = 0;

    // Set up the node stack and initialize the root node
    stack<Vertex> parentage;
    parentage.push( add_vertex(m_graph) );
    m_graph[parentage.top()] = MakeNode(KW_RootNode,KW_RootNode);

    // Arguments to be associated with the next keyword found
    vector<PdfVariant> args;

    while ( ( readToken = contentsTokenizer.ReadNext(t, kwText, var) ) )
    {
        ++tokenNumber;
        if (t == ePdfContentsType_Variant)
        {
            // arguments come before operators, but we want to group them up before
            // their operator.
            args.push_back(var);
        }
        else if (t == ePdfContentsType_Keyword)
        {
            const KWInfo & ki ( findKwByName(kwText) );
            if (ki.kt != KT_Closing)
            {
                // We're going to need a new vertex, so make sure we have one ready.
                Vertex v = add_vertex( m_graph );
                // Switch any waiting arguments into the new node's data.
                m_graph[v].first.GetArgs().swap( args );
                assert(!args.size());

                if (ki.kw == KW_Unknown)
                {
                    // No idea what this keyword is. We have to assume it's an ordinary
                    // one, possibly with arguments, and just push it in as a node at the
                    // current level.
                    assert(!m_graph[v].first.IsDefined());
                    m_graph[v].first.SetKw( string(kwText) );
                    add_edge( parentage.top(), v, m_graph );
                    assert( m_graph[v].first.GetKwId() == ki.kw );
                    assert( m_graph[v].first.GetKwString() == kwText );
                }
                else if (ki.kt == KT_Standalone)
                {
                    // Plain operator, shove it in the newly reserved vertex (which might already contain
                    // arguments) and add an edge from the top to it.
                    assert(ki.kw != KW_Undefined && ki.kw != KW_Unknown && ki.kw != KW_RootNode );
                    assert(!m_graph[v].first.IsDefined());
                    m_graph[v].first.SetKw( ki.kw );
                    add_edge( parentage.top(), v, m_graph );
                    assert( m_graph[v].first.GetKwId() == ki.kw );
                    assert( m_graph[v].first.GetKwString() == kwText );
                }
                else if (ki.kt == KT_Opening)
                {
                    PrintStack(m_graph, parentage, "OS: ");
                    assert(ki.kw != KW_Undefined && ki.kw != KW_Unknown && ki.kw != KW_RootNode );
                    assert(!m_graph[v].first.IsDefined());
                    m_graph[v].first.SetKw( ki.kw );
                    // add an edge from the current top to it
                    add_edge( parentage.top(), v, m_graph );
                    // and push it to the top of the parentage stack
                    parentage.push( v );
                    assert( m_graph[v].first.GetKwId() == ki.kw );
                    assert( m_graph[v].first.GetKwString() == kwText );
                    PrintStack(m_graph, parentage, "OF: ");
                }
                else
                {
                    assert(false);
                }
            }
            else if (ki.kt == KT_Closing)
            {
                // This keyword closes a context. The top of the parentage tree should
                // be a node whose KWInstance is the matching opening keyword. We'll check
                // that, then set the second KWInstance appropriately.
                PrintStack(m_graph, parentage, "CS: ");
                assert(ki.kw != KW_Undefined && ki.kw != KW_Unknown && ki.kw != KW_RootNode );
                // Get a reference to the node data for the current parent
                NodeData & n ( m_graph[parentage.top()] );
                PODOFO_RAISE_LOGIC_IF( n.second.IsDefined(), "Closing already closed group" );
                // Ensure that the opening keyword therein is one that this closing keyword is
                // a valid match for
                PdfContentStreamKeyword expectedCloseKw = n.first.GetKwInfo().kwClose;
                // Ensure there aren't any args to the close kw
                assert(!args.size());
                // and handle the close matching
                if ( ki.kw != expectedCloseKw )
                {
                    // Some PDFs, even Adobe ones, place close operators
                    // in the wrong order. We'll do some lookahead to see
                    // if we can fix things up before we hit a non-close
                    // operator.
                    if ( !closeFixup( m_graph, parentage, contentsTokenizer, ki  ) )
                    {
                        string err = formatMismatchError(m_graph, parentage, tokenNumber, ki.kw, expectedCloseKw);
                        PODOFO_RAISE_ERROR_INFO( ePdfError_InvalidContentStream, err.c_str() );
                    }
                }
                else
                {
                    n.second.SetKw( ki.kw );
                    // Our associated operator is now on the top of the
                    // parentage stack. Since its scope has ended, it should
                    // also be popped.
                    parentage.pop();
                }
                PrintStack(m_graph, parentage, "CF: ");
            }
            else
            {
                assert(false);
            }
        }
        else
        {
            assert(false);
        }
    }

    PODOFO_RAISE_LOGIC_IF( args.size(), "Stream ended with unconsumed arguments!" );

    PODOFO_RAISE_LOGIC_IF( parentage.size() != 1, "Stream failed to close all levels" );

}