C++ Vec3d类代码示例

void virtuose::fillSpeed(VRPhysics* p, float* to, VRPhysics* origin) {
    Vec3d vel = p->getLinearVelocity();
    if (origin!=0) vel -= origin->getLinearVelocity();
    to[0] = vel.z();
    to[1] = vel.x();
    to[2] = vel.y();
    Vec3d ang = p->getAngularVelocity();
    if (origin!=0) ang -= origin->getAngularVelocity();
    to[3] = ang.z();
    to[4] = ang.x();
    to[5] = ang.y();
}

std::vector<double> Cuboid::getCoordinate( Point v )
{
	std::vector<double> coords(3);

	// Map points to uniform box coordinates
	QSurfaceMesh currBoxMesh = getGeometry();
	QSurfaceMesh currUniformBoxMesh = getBoxGeometry(currBox, true);

	v = MeanValueCooridnates::point(
		MeanValueCooridnates::weights(v, &currBoxMesh), &currUniformBoxMesh);

	Vec3d pos = getCoordinatesInUniformBox(currBox, v);

	coords[0] = pos.x();
	coords[1] = pos.y();
	coords[2] = pos.z();

	return coords;
}

 void Camera::init(){
     //Matrix4d::zero(transformMatrix);
     // initalize view center
     Vec3d orient = (focus-position);
     orient.normalize();
     viewCenter = position + orient * distance;
     // initialzie horiVec and vertVec
     if ((fabs(orient.y) > Epsilon) || (fabs(orient.x) > Epsilon)){
         horiVec = Vec3d(-orient.y, orient.x, 0);
     }
     else{
         horiVec = Vec3d(-orient.z, 0, orient.x);
     }
     vertVec = Vec3d::cross(horiVec, orient);
     vertVec.normalize();
     horiVec.normalize();
     vertVec *= distance * tan(vAngle/2);
     horiVec *= distance * tan(hAngle/2);
 }

Group Transform::apply(const Group& g)
{
  std::vector<Vec3d> vv;
  Vec3d vtmp;
  double x1, y1, z1;
  int s = g.n_atom_;
  for (int i = 0; i < s; ++i) {
    vtmp = g.coordinates_[i];
    x1 = rotv1_ * vtmp;
    y1 = rotv2_ * vtmp;
    z1 = rotv3_ * vtmp;
    vtmp.set_coordinate(x1, y1, z1);
    vtmp += translation_;
    vv.push_back(vtmp);
  }
  Group gtmp(vv);

  return gtmp;
}

//--------------------------------------------------------------------------------------------------
/// 
//--------------------------------------------------------------------------------------------------
TEST(VariantTest, TypeVec3d)
{
    const Vec3d val(1.0, -2000.1234, 3000.6789);
    Variant var(val);
    ASSERT_EQ(Variant::VEC3D, var.type());
    ASSERT_TRUE(var.isValid());

    Vec3d v = var.getVec3d();
    ASSERT_EQ(val.x(), v.x());
    ASSERT_EQ(val.y(), v.y());
    ASSERT_EQ(val.z(), v.z());
}

Vec3d rectToSpherical(const Vec3d& v)
{
    double r = v.length();
    double theta = atan2(v.y, v.x);
    if (theta < 0)
        theta = theta + 2 * PI;
    double phi = asin(v.z / r);

    return Vec3d(theta, phi, r);
}

 void add_point(const Vec3d& point){
     uchar b = ~bits_; //Flip bits
     b &= -b; //Last set (available) bit
     uchar pos = s_pos[b]; //Get the bit position from the lookup table
     last_sb_ = pos;
     bits_ |= b; //Insert the new bit
     ++size_;
     p_[pos] = point;
     double l2 = point.length2();
     if(l2 > max_vert2) max_vert2 = l2;
 }

/*
 * Check existance of obstacles. The route is specified by 
 * source and destination. If targetBurdenLoc is given, it is
 * excluded from obstacles. If targetBurdenLoc==null, ignored.
 */
bool SampleBase::checkAllConflict(const Vec3d& src,const Vec3d& dest,const Vec3d& targetBurdenLoc) {
    vector<Vec3d> array = burdenSet.getVectors();
    for (vector<Vec3d>::iterator itr = array.begin(); itr != array.end(); itr++) {
        Vec3d v = (*itr);
        if (v == src)
            continue;
        if (v == dest)
            continue;
        if (targetBurdenLoc.x<1000 && v.epsilonEquals(targetBurdenLoc,1.0))
            continue;
        if (checkConflict(src,dest,v,1.5)) {
            return true;
        }
    }
    if (checkConflict(src,dest,obstacle1,3.0))
        return true;
    if (checkConflict(src,dest,obstacle2,3.0))
        return true;
    return false;
}

Transformation3d :: Transformation3d (const Vec3d & translate)
{
  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      lin[i][j] = 0;
  for (int i = 0; i < 3; i++)
    {
      offset[i] = translate.X(i+1);
      lin[i][i] = 1;
    }
}

		void SofaHAPIHapticsDevice::draw(const sofa::core::visual::VisualParams* vparams)
		{
			if (!device.get()) return;
			if(drawDevice.getValue())
			{
				// TODO
				HAPI::HAPIHapticsDevice::DeviceValues dv = device->getDeviceValues();
				/// COMPUTATION OF THE virtualTool 6D POSITION IN THE World COORDINATES
				Vec3d pos = conv(dv.position);
				Vec3d force = conv(dv.force);
				Quat quat = conv(dv.orientation);
				quat.normalize();
				Transform baseDevice_H_endDevice(pos*data.scale, quat);
				Transform world_H_virtualTool = data.world_H_baseDevice * baseDevice_H_endDevice * data.endDevice_H_virtualTool;
				Vec3d wpos = world_H_virtualTool.getOrigin();

				vparams->drawTool()->setLightingEnabled(true); //Enable lightning
				if (drawHandleSize.getValue() == 0.0f)
				{
					std::vector<Vec3d> points;
					points.push_back(wpos);
					vparams->drawTool()->drawSpheres(points, 1.0f, sofa::defaulttype::Vec<4,float>(0,0,1,1));
				}
				else
				{
					Vec3d wposH = wpos + data.world_H_baseDevice.projectVector( baseDevice_H_endDevice.projectVector(Vec3d(0.0,0.0,drawHandleSize.getValue())));
					vparams->drawTool()->drawArrow(wposH, wpos, drawHandleSize.getValue()*0.05f, sofa::defaulttype::Vec<4,float>(0,0,1,1));
				}
				if (force.norm() > 0 && drawForceScale.getValue() != 0.0f)
				{
					//std::cout << "F = " << force << std::endl;
					Vec3d fscaled = force*(drawForceScale.getValue()*data.scale);
					Transform baseDevice_H_endDeviceF(pos*data.scale+fscaled, quat);
					Transform world_H_virtualToolF = data.world_H_baseDevice * baseDevice_H_endDeviceF * data.endDevice_H_virtualTool;
					Vec3d wposF = world_H_virtualToolF.getOrigin();
					vparams->drawTool()->drawArrow(wpos, wposF, 0.1f, sofa::defaulttype::Vec<4,float>(1,0,0,1));
				}

				vparams->drawTool()->setLightingEnabled(false); //Disable lightning
			}
		}

void ParticleSystem::constraintSpring( double dt )
{
	for (int iter = 0; iter < 20; iter++)
	{
		for (size_t i = 0; i < springs.size(); i++)
		{
			if (springs[i].type == Spring::SPRING_BEND)
			{
				continue;
			}
			int p1 = springs[i].p1;
			int p2 = springs[i].p2;
			double l0 = springs[i].l0;

			Vec3d dx = pts[p1].cp - pts[p2].cp;
			Vec3d c = (pts[p1].cp + pts[p2].cp) / 2;
			double d = sqrt(dx.ddot(dx));

			if (d > maxSpringLen * l0)
			{
				pts[p1].cp = c + dx * maxSpringLen * l0 / (d * 2);
				pts[p2].cp = c - dx * maxSpringLen * l0 / (d * 2);
			}
			if (d < minSpringLen * l0)
			{
				pts[p1].cp = c + dx * minSpringLen * l0 / (d * 2);
				pts[p2].cp = c - dx * minSpringLen * l0 / (d * 2);
			}
		}
	}

	// recalculate the velocity
	for (size_t i = 0; i < pts.size(); i++)
	{
		if (pts[i].isPinned)
		{
			pts[i].cp = pts[i].op;
		}
		pts[i].v = (pts[i].cp - pts[i].op) / dt;
	}
}

namespace BlockWorld {

    static Vec3d scaling;
    static Vec3d pos0;
    static Mat3d rotations[6];

    void drawBlock( Uint16 shape, Uint8 orientation, Uint8 ix, Uint8 iy, Uint8 iz ){
        Mat3d rotmat = rotations[ orientation & 0b00011111 ];
        rotmat.a.mul( scaling.x );
        rotmat.b.mul( scaling.y );
        rotmat.c.mul( scaling.z );
        if (orientation & 0b10000000) rotmat.a.mul( -1.0d );
        if (orientation & 0b01000000) rotmat.b.mul( -1.0d );
        if (orientation & 0b00100000) rotmat.c.mul( -1.0d );
        Vec3d pos; pos.set( ix*scaling.x+pos0.x, iy*scaling.y+pos0.y, iz*scaling.z+pos0.z );
        float glMat[16];
        glPushMatrix ( );
        /*
        printf( " pos (%3.3f,%3.3f,%3.3f) \n", pos.x, pos.y, pos.z );
        printf( " a   (%3.3f,%3.3f,%3.3f) \n", rotmat.a.x, rotmat.a.x, rotmat.a.x );
        printf( " b   (%3.3f,%3.3f,%3.3f) \n", rotmat.b.x, rotmat.b.x, rotmat.b.x );
        printf( " c   (%3.3f,%3.3f,%3.3f) \n", rotmat.c.x, rotmat.c.x, rotmat.c.x );
        */
        Draw3D::toGLMat( pos, rotmat, glMat );
        glMultMatrixf( glMat );
        glCallList  ( shape );
        glPopMatrix ( );
    }

    void buildRotations( const Mat3d& rot ){
        rotations[0].set( rot.a, rot.b, rot.c );
        rotations[1].set( rot.a, rot.c, rot.b );
        rotations[2].set( rot.b, rot.a, rot.c );
        rotations[3].set( rot.b, rot.c, rot.a );
        rotations[4].set( rot.c, rot.a, rot.b );
        rotations[5].set( rot.c, rot.b, rot.a );
    }

    void setupBlockWorld( const Vec3d& pos0_, const Vec3d& scaling_, const Mat3d& rot ){
        pos0   .set( pos0_    );
        scaling.set( scaling_ );
        buildRotations( rot );
        /*
        printf( " pos      (%3.3f,%3.3f,%3.3f) \n", pos0.x, pos0.y, pos0.z );
        //printf( " scaling_ (%3.3f,%3.3f,%3.3f) \n", scaling_.x, scaling_.y, scaling_.z );
        printf( " scaling  (%3.3f,%3.3f,%3.3f) \n", scaling.x,  scaling.y, scaling.z );
        printf( " a        (%3.3f,%3.3f,%3.3f) \n", rotations[0].a.x, rotations[0].a.y, rotations[0].a.z );
        printf( " b        (%3.3f,%3.3f,%3.3f) \n", rotations[0].b.x, rotations[0].b.y, rotations[0].b.z );
        printf( " c        (%3.3f,%3.3f,%3.3f) \n", rotations[0].c.x, rotations[0].c.y, rotations[0].c.z );
        */
    }

};

bool Geometry::intersect(const ray&r, isect&i) const
{
    // Transform the ray into the object's local coordinate space
    Vec3d pos = transform->globalToLocalCoords(r.getPosition());
    Vec3d dir = transform->globalToLocalCoords(r.getPosition() + r.getDirection()) - pos;
    double length = dir.length();
    dir /= length;

    ray localRay( pos, dir, r.type() );

    if (intersectLocal(localRay, i)) {
        // Transform the intersection point & normal returned back into global space.
        i.N = transform->localToGlobalCoordsNormal(i.N);
        i.t /= length;

        return true;
    } else {
        return false;
    }

}

int ClosedPolygon::insertIndexedPoint(const Vec3d &p)
{
	int vIndex = -1;

	kdres * findP = points.nearest3f(p.x(), p.y(), p.z());

	if(findP)
	{
		double * pos = findP->riter->item->pos;

		Vec3d closestPoint(pos[0], pos[1], pos[2]);

		double dist = (closestPoint - p).norm();

		if(dist < closedPolyEpsilon)
		{
			vIndex = findP->riter->item->index;
			kd_res_free(findP);
			return vIndex;
		}
	}

	vIndex = lastVertexIndex;
	allPoints[vIndex] = p;

	points.insert3f(p.x(), p.y(), p.z(), lastVertexIndex++);

	return vIndex;
}

bool Triangle::angleCriterion( const double &minCosAngle, 
	const double &maxCosAngle )
{
	Vec3d ab = m_vertices[1].m_xyz - m_vertices[0].m_xyz;
	Vec3d bc = m_vertices[2].m_xyz - m_vertices[1].m_xyz;
	Vec3d ac = m_vertices[2].m_xyz - m_vertices[0].m_xyz;

	double lenAB = ab.dot(ab);
	double lenBC = bc.dot(bc);
	double lenAC = ac.dot(ac);

	double maxLen = lenAB, minLen = lenAB;
	double lenMaxE0 = lenBC, lenMaxE1 = lenAC;
	double lenMinE0 = lenBC, lenMinE1 = lenAC;
	Vec3d maxE0 = bc, maxE1 = ac, minE0 = bc, minE1 = ac;

	bool maxFlag = true, minFlag = true;
	if (maxCosAngle > COS180 && maxCosAngle <  COS60)
	{
		if (maxLen < lenBC)
		{
			maxLen = lenBC;
			lenMaxE0 = lenAB;
			lenMaxE1 = lenAC;
			maxE0 = ab;
			maxE1 = ac;
		}
		if (maxLen < lenAC)
		{
			lenMaxE0 = lenAB;
			lenMaxE1 = lenBC;
			maxE0 = -ab;
			maxE1 = bc;
		}
		maxFlag = maxE0.dot(maxE1) > sqrt(lenMaxE0) 
			* sqrt(lenMaxE1) * maxCosAngle;
	}
	if (minCosAngle < COS0 && minCosAngle > COS60)
	{
		if (minLen > lenBC)
		{
			minLen = lenBC;
			lenMinE0 = lenAB;
			lenMinE1 = lenAC;
			minE0 = ab;
			minE1 = ac;
		}
		if (minLen > lenAC)
		{
			lenMinE0 = lenAB;
			lenMinE1 = lenBC;
			minE0 = -ab;
			minE1 = bc;
		}
		minFlag = minE0.dot(minE1) < sqrt(lenMinE0) 
			* sqrt(lenMinE1) * minCosAngle;
	}
	
	return minFlag && maxFlag;
}