C++ vec3::z方法代码示例

void ewol::compositing::Drawing::lineTo(const vec3& _dest) {
	resetCount();
	internalSetColor(m_color);
	//EWOL_VERBOSE("DrawLine : " << m_position << " to " << _dest);
	if (m_position.x() == _dest.x() && m_position.y() == _dest.y() && m_position.z() == _dest.z()) {
		//EWOL_WARNING("Try to draw a line width 0");
		return;
	}
	//teta = tan-1(oposer/adjacent)
	float teta = 0;
	if (m_position.x() <= _dest.x()) {
		teta = atan((_dest.y()-m_position.y())/(_dest.x()-m_position.x()));
	} else {
		teta = M_PI + atan((_dest.y()-m_position.y())/(_dest.x()-m_position.x()));
	}
	if (teta < 0) {
		teta += 2*M_PI;
	} else if (teta > 2*M_PI) {
		teta -= 2*M_PI;
	}
	//EWOL_DEBUG("teta = " << (teta*180/(M_PI)) << " deg." );
	float offsety = sin(teta-M_PI/2) * (m_thickness/2);
	float offsetx = cos(teta-M_PI/2) * (m_thickness/2);
	setPoint(vec3(m_position.x() - offsetx, m_position.y() - offsety, m_position.z()) );
	setPoint(vec3(m_position.x() + offsetx, m_position.y() + offsety, m_position.z()) );
	setPoint(vec3(_dest.x()      + offsetx, _dest.y()      + offsety, m_position.z()) );
	
	setPoint(vec3(_dest.x()      + offsetx, _dest.y()      + offsety, _dest.z()) );
	setPoint(vec3(_dest.x()      - offsetx, _dest.y()      - offsety, _dest.z()) );
	setPoint(vec3(m_position.x() - offsetx, m_position.y() - offsety, _dest.z()) );
	// update the system position :
	m_position = _dest;
}

void DBaseBatchPlanner::writeRawToFile( int i, int j, const std::vector<RawScanPoint> &rawData, 
					vec3 loc, vec3 dir, vec3 up)
{
	QString filename = QString(mScanDirectory) + mObject->getName() + QString("_raw");
	QString n;
	n.setNum(i);
	filename = filename + n;
	n.setNum(j);
	filename = filename + QString("_") + n + QString(".txt");

	FILE *f = fopen(filename.latin1(),"w");
	if (!f) {
		DBGAF(mLogStream,"Failed to open file " << filename.latin1());
		fprintf(stderr,"Failed to open scan file\n");
		return;
	}

	fprintf(f,"%f %f %f\n",loc.x(), loc.y(), loc.z());
	fprintf(f,"%f %f %f\n",dir.x(), dir.y(), dir.z());
	fprintf(f,"%f %f %f\n",up.x(), up.y(), up.z());
	fprintf(f,"%d\n",(int)rawData.size());

	for (int k=0; k<(int)rawData.size(); k++) {
		fprintf(f,"%f %f ",rawData[k].hAngle, rawData[k].vAngle);
		fprintf(f,"%f %f %f ",rawData[k].dx, rawData[k].dy, rawData[k].dz);
		fprintf(f,"%f\n",rawData[k].distance);
	}

	writeSolutionsToFile(f);
	fclose(f);
}

    static void lookAt(const vec3<Type>& eye, const vec3<Type>& center, const vec3<Type>& up)
    {
        const vec3<Type> forward = (center - eye).normalized();
        const vec3<Type> side = forward.cross(up).normalized();
        const vec3<Type> upVector = side.cross(forward);

        matrix4<Type> m;
        m.m_data[0][0] = side.x();
        m.m_data[1][0] = side.y();
        m.m_data[2][0] = side.z();
        m.m_data[3][0] = -side.dot(eye);
        m.m_data[0][1] = upVector.x();
        m.m_data[1][1] = upVector.y();
        m.m_data[2][1] = upVector.z();
        m.m_data[3][1] = -upVector.dot(eye);
        m.m_data[0][2] = -forward.x();
        m.m_data[1][2] = -forward.y();
        m.m_data[2][2] = -forward.z();
        m.m_data[3][2] = -forward.dot(eye);
        m.m_data[0][3] = 0;
        m.m_data[1][3] = 0;
        m.m_data[2][3] = 0;
        m.m_data[3][3] = 1;

        return m;
    }

      /** \brief Expand the boundary.
       *
       * \param other Other boundary.
       */
      void expand(const math::rect3<T> &other)
      {
        if(other.x1() < m_pnt_min.x())
        {
          m_pnt_min.x() = other.x1();
        }
        if(other.x2() < m_pnt_max.x())
        {
          m_pnt_max.x() = other.x2();
        }

        if(other.y1() < m_pnt_min.y())
        {
          m_pnt_min.y() = other.y1();
        }
        if(other.y2() < m_pnt_max.y())
        {
          m_pnt_max.y() = other.y2();
        }

        if(other.z1() < m_pnt_min.z())
        {
          m_pnt_min.z() = other.z1();
        }
        if(other.z2() < m_pnt_max.z())
        {
          m_pnt_max.z() = other.z2();
        }
      }

/*!
  Creates a 6x6 matrix, \a transformMat, that transforms a wrench expressed
  in one coordinate system to wrench expressed in another.  \a T is the
  transform, and \a p is the new torque origin expressed within the new
  coordinate system.
*/
void buildForceTransform(transf &T,vec3 &p,double *transformMat)
{
  static int j,k;
  static double R[9];
  static double crossMat[9];
  static double Rcross[9];
  static vec3 radius;

  R[0] = T.affine().element(0,0); 
  R[1] = T.affine().element(0,1); 
  R[2] = T.affine().element(0,2); 
  
  R[3] = T.affine().element(1,0); 
  R[4] = T.affine().element(1,1); 
  R[5] = T.affine().element(1,2); 
  
  R[6] = T.affine().element(2,0); 
  R[7] = T.affine().element(2,1); 
  R[8] = T.affine().element(2,2); 
/*
  R[0] = T.affine().element(0,0); 
  R[1] = T.affine().element(1,0); 
  R[2] = T.affine().element(2,0); 
  
  R[3] = T.affine().element(0,1); 
  R[4] = T.affine().element(1,1); 
  R[5] = T.affine().element(2,1); 
  
  R[6] = T.affine().element(0,2); 
  R[7] = T.affine().element(1,2); 
  R[8] = T.affine().element(2,2); 
*/ 

  for (j=0;j<9;j++)
    if (fabs(R[j]) < MACHINE_ZERO) R[j] = 0.0;
    else if (R[j] > 1.0 - MACHINE_ZERO) R[j] = 1.0;
    else if (R[j] < -1.0 + MACHINE_ZERO) R[j] = -1.0;
      
  radius = T.translation() - p;
	
  crossMat[0]=0.0;       crossMat[3]=-radius.z();crossMat[6]=radius.y();
  crossMat[1]=radius.z();crossMat[4]=0.0;        crossMat[7]=-radius.x();
  crossMat[2]=-radius.y();crossMat[5]= radius.x();crossMat[8]=0.0;

  //original graspit
  //dgemm("N","N",3,3,3,1.0,R,3,crossMat,3,0.0,Rcross,3);

  // mtc: new version, I believe this is the correct one
  dgemm("N","N",3,3,3,1.0,crossMat,3,R,3,0.0,Rcross,3);
	
  fillMatrixBlock(R,3,0,0,2,2,transformMat,6);
  for (j=3;j<6;j++)
    for (k=0;k<3;k++) 
      transformMat[6*j+k] = 0.0;
  fillMatrixBlock(Rcross,3,3,0,5,2,transformMat,6);
  fillMatrixBlock(R,3,3,3,5,5,transformMat,6);
}

void System::minimumImageCriterion(vec3 &pos)
{
    if (pos.x() > m_systemSize.x() * 0.5) pos[0] = pos[0] - m_systemSize.x();
    else if (pos.x() <= -m_systemSize.x() * 0.5) pos[0] = pos[0] + m_systemSize.x();
    if (pos.y() > m_systemSize.y() * 0.5) pos[1] = pos[1] - m_systemSize.y();
    else if (pos.y() <= -m_systemSize.y() * 0.5) pos[1] = pos[1] + m_systemSize.y();
    if (pos.z() > m_systemSize.z() * 0.5) pos[2] = pos[2] - m_systemSize.z();
    else if (pos.z() <= -m_systemSize.z() * 0.5) pos[2] = pos[2] + m_systemSize.z();
}

 /** \brief Check whether this rect is completely outside another.
  *
  * Note that rectangles touching on one side are not considered
  * intersecting.
  *
  * \param other Other rectangle.
  * \return True if no overlap of any kind, false otherwise.
  */
 bool isOutside(const math::rect3<T> &other) const
 {
   return ((m_pnt_max.x() <= other.m_pnt_min.x()) ||
       (m_pnt_min.x() >= other.m_pnt_max.x()) ||
       (m_pnt_max.y() <= other.m_pnt_min.y()) ||
       (m_pnt_min.y() >= other.m_pnt_max.y()) ||
       (m_pnt_max.z() <= other.m_pnt_min.z()) ||
       (m_pnt_min.z() >= other.m_pnt_max.z()));
 }

RGBColor convertColor( vec3& clr){
    int r,g,b;
    clr.setX( std::max( std::min( clr.x(), 1.0), 0.0 ) );
    clr.setY( std::max( std::min( clr.y(), 1.0), 0.0 ) );
    clr.setZ( std::max( std::min( clr.z(), 1.0), 0.0 ) );
    r=(int)(255*clr.x());
    g=(int)(255*clr.y());
    b=(int)(255*clr.z());
    return RGBColor(r, g, b);
}

void TouchscreenButton::DebugRender(const vec3& position,
                                    const vec3& texture_size,
                                    fplbase::Renderer& renderer) const {
#if defined(DEBUG_RENDER_BOUNDS)
    if (debug_shader_ && draw_bounds_) {
        const vec2 window_size = vec2(renderer.window_size());
        static const float kButtonZDepth = 0.0f;
        static const fplbase::Attribute kFormat[] = {fplbase::kPosition3f,
                                                     fplbase::kEND
                                                    };
        static const unsigned short kIndices[] = {0, 1, 1, 3, 2, 3, 2, 0};
        const vec3 bottom_left = position - (texture_size / 2.0f);
        const vec3 top_right = position + (texture_size / 2.0f);

        // vertex format is [x, y, z]
        float vertices[] = {
            bottom_left.x(), bottom_left.y(), bottom_left.z(), top_right.x(),
            bottom_left.y(), bottom_left.z(), bottom_left.x(), top_right.y(),
            top_right.z(),   top_right.x(),   top_right.y(),   top_right.z(),
        };
        renderer.set_color(vec4(1.0f, 0.0f, 1.0f, 1.0f));
        debug_shader_->Set(renderer);
        fplbase::Mesh::RenderArray(fplbase::Mesh::kLines, 8,
                                   kFormat, sizeof(float) * 3,
                                   reinterpret_cast<const char*>(vertices),
                                   kIndices);

        renderer.set_color(vec4(1.0f, 1.0f, 0.0f, 1.0f));
        debug_shader_->Set(renderer);
        static unsigned short indicesButtonDef[] = {1, 0, 1, 2, 2, 3, 3, 0};
        float verticesButtonDef[] = {
            button_def()->top_left()->x() * window_size.x(),
            button_def()->top_left()->y() * window_size.y(),
            kButtonZDepth,
            button_def()->top_left()->x() * window_size.x(),
            button_def()->bottom_right()->y() * window_size.y(),
            kButtonZDepth,
            button_def()->bottom_right()->x() * window_size.x(),
            button_def()->bottom_right()->y() * window_size.y(),
            kButtonZDepth,
            button_def()->bottom_right()->x() * window_size.x(),
            button_def()->top_left()->y() * window_size.y(),
            kButtonZDepth,
        };
        fplbase::Mesh::RenderArray(fplbase::Mesh::kLines, 8, kFormat,
                                   sizeof(float) * 3,
                                   reinterpret_cast<const char*>(verticesButtonDef),
                                   indicesButtonDef);
    }
#else
    (void)position;
    (void)texture_size;
    (void)renderer;
#endif  // DEBUG_RENDER_BOUNDS
}

void boxSize(const position &p, vec3 &min, vec3 &max, 
			 const vec3 &x, const vec3 &y, const vec3 &z, double tolerance)
{
	vec3 d = p - position::ORIGIN;
	double dx = d % x;
	double dy = d % y;
	double dz = d % z;		
	if ( dx + tolerance > max.x() ) max.x() = dx + tolerance;
	if ( dy + tolerance > max.y() ) max.y() = dy + tolerance;
	if ( dz + tolerance > max.z() ) max.z() = dz + tolerance;
	if ( dx - tolerance < min.x() ) min.x() = dx - tolerance;
	if ( dy - tolerance < min.y() ) min.y() = dy - tolerance;
	if ( dz - tolerance < min.z() ) min.z() = dz - tolerance;
}

// Area of a triangle in 3d.
float area(vec3 &a, vec3 &b, vec3 &c) {
    float xA = a.x(), yA = a.y(), zA = a.z();
    float xB = b.x(), yB = b.y(), zB = b.z();
    float xC = c.x(), yC = c.y(), zC = c.z();
    float det0 = det3(xA, xB, xC,
                      yA, yB, yC,
                      1 ,  1,  1);
    float det1 = det3(yA, yB, yC,
                      zA, zB, zC,
                      1 ,  1,  1);
    float det2 = det3(zA, zB, zC,
                      xA, xB, xC,
                      1 ,  1,  1);
    return 0.5f * sqrtf( det0 * det0 + det1 * det1 + det2 * det2 );
}

 virtual color value(float u, float v, vec3 const &P) const 
 {
     vec3::type d = sin(scale * P.x()) * cos(scale * P.y()) * sin(scale * P.z());
     if (d < 0)
         return odd->value(u, v, P);
     return even->value(u, v, P); 
 }

    /** Updates the AABB to contain the given point. */
    void addPoint(const vec3& p) 
    {
      if (isNull())
      {
        mMax = p;
        mMin = p;
        return;
      }

      if ( mMax.x() < p.x() ) mMax.x() = p.x();
      if ( mMax.y() < p.y() ) mMax.y() = p.y();
      if ( mMax.z() < p.z() ) mMax.z() = p.z();
      if ( mMin.x() > p.x() ) mMin.x() = p.x();
      if ( mMin.y() > p.y() ) mMin.y() = p.y();
      if ( mMin.z() > p.z() ) mMin.z() = p.z();
    }

void HuboPlus::computeGroundReaction(const real m,
				     const vec3& comPos,
				     const vec3& comAccel,
				     const Transform3& footXform,
				     vec3* force,
				     vec3* torque) const {

  assert(comAccel.z() == 0);

  // COM in frame of foot
  vec3 cp = footXform.transformInv(comPos);

  // COM accel in frame of foot
  vec3 ca = footXform.rotInv() * comAccel;
  ca[2] += g; // tack on gravity to the Z component

  // F = ma
  vec3 f = m*ca;


  if (force)  { *force = f; }
  if (torque) { 
    // moment arm for torque from foot
    vec3 r = -cp;
    *torque = vec3::cross(r, f); 
  }

}

static void getUV(float& u, float& v, const vec3& p)
{
    float phi = asin(p.y());
    float theta = atan(p.x() / p.z());

    u = (theta + _MATH_PI1_2) / _MATH_PI;
    v = (phi + _MATH_PI1_2) / _MATH_PI;
}