C++ Vec3d::y方法代码示例

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

//--------------------------------------------------------------------------------------------------
///  Test bounding box to see if it's outside the frustum or not
///
/// \return true if outside or exactly on the boundary. false if inside.
//--------------------------------------------------------------------------------------------------
bool Frustum::isOutside(const BoundingBox& bbox) const
{
    CVF_ASSERT(m_planes.size() == 6);

    const Vec3d& boxMin = bbox.min();
    const Vec3d& boxMax = bbox.max();

    Vec3d point;
    Vec3d planeNormal;

    std::map<int, Plane>::const_iterator it;
    for (it = m_planes.begin(); it != m_planes.end(); it++)
    {
        planeNormal = it->second.normal();

        point.x() = (planeNormal.x() <= 0.0) ? boxMin.x() : boxMax.x();
        point.y() = (planeNormal.y() <= 0.0) ? boxMin.y() : boxMax.y();
        point.z() = (planeNormal.z() <= 0.0) ? boxMin.z() : boxMax.z();

        if (it->second.distanceSquared(point) < 0.0)
        {
            return true;
        }
    }

    return false;
}

void SimpleDraw::DrawCircle( const Vec3d& center, double radius, const Vec4d& c, const Vec3d& n, float lineWidth )
{
	Vec3d startV(0,0,0);

	// Find orthogonal start vector
	if ((abs(n.y()) >= 0.9f * abs(n.x())) && 
		abs(n.z()) >= 0.9f * abs(n.x())) startV = Vec3d(0.0f, -n.z(), n.y());
	else if ( abs(n.x()) >= 0.9f * abs(n.y()) && 
		abs(n.z()) >= 0.9f * abs(n.y()) ) startV = Vec3d(-n.z(), 0.0f, n.x());
	else startV = Vec3d(-n.y(), n.x(), 0.0f);

	int segCount = 20;
	double theta = 2.0 * M_PI / segCount;

	glDisable(GL_LIGHTING);
	glLineWidth(lineWidth);
	glColor4dv(c);

	glBegin(GL_LINE_LOOP);
	for(int i = 0; i < segCount; i++){
		glVertex3dv(center + startV * radius );
		ROTATE_VEC(startV, theta, n);
	}
	glEnd();

	glEnable(GL_LIGHTING);
}

pair<ofVec3f, ofVec3f> VDB::bbox() {
	math::CoordBBox bbox = grid->evalActiveVoxelBoundingBox();
	Coord minC = bbox.getStart();
	Coord maxC = bbox.getEnd();
	Vec3d minPt = grid->indexToWorld(minC);
	Vec3d maxPt = grid->indexToWorld(maxC);
	return make_pair(ofVec3f(minPt.x(), minPt.y(), minPt.z()), ofVec3f(maxPt.x(), maxPt.y(), maxPt.z()));
}

//--------------------------------------------------------------------------------------------------
/// 
//--------------------------------------------------------------------------------------------------
void BoundingBox::add(const Vec3d& point)
{
    if (point.x() < m_min.x()) m_min.x() = point.x();
    if (point.y() < m_min.y()) m_min.y() = point.y();
    if (point.z() < m_min.z()) m_min.z() = point.z();

    if (point.x() > m_max.x()) m_max.x() = point.x();
    if (point.y() > m_max.y()) m_max.y() = point.y();
    if (point.z() > m_max.z()) m_max.z() = point.z();
}

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

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

// Make a rotation Quat which will rotate vec1 to vec2
// Generally take adot product to get the angle between these
// and then use a cross product to get the rotation axis
// Watch out for the two special cases of when the vectors
// are co-incident or opposite in direction.
void Quat::makeRotate_original( const Vec3d& from, const Vec3d& to )
{
    const value_type epsilon = 0.0000001;

    value_type length1  = from.length();
    value_type length2  = to.length();

    // dot product vec1*vec2
    value_type cosangle = from*to/(length1*length2);

    if ( fabs(cosangle - 1) < epsilon )
    {
        OSG_INFO<<"*** Quat::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl;

        // cosangle is close to 1, so the vectors are close to being coincident
        // Need to generate an angle of zero with any vector we like
        // We'll choose (1,0,0)
        makeRotate( 0.0, 0.0, 0.0, 1.0 );
    }
    else
    if ( fabs(cosangle + 1.0) < epsilon )
    {
        // vectors are close to being opposite, so will need to find a
        // vector orthongonal to from to rotate about.
        Vec3d tmp;
        if (fabs(from.x())<fabs(from.y()))
            if (fabs(from.x())<fabs(from.z())) tmp.set(1.0,0.0,0.0); // use x axis.
            else tmp.set(0.0,0.0,1.0);
        else if (fabs(from.y())<fabs(from.z())) tmp.set(0.0,1.0,0.0);
        else tmp.set(0.0,0.0,1.0);

        Vec3d fromd(from.x(),from.y(),from.z());

        // find orthogonal axis.
        Vec3d axis(fromd^tmp);
        axis.normalize();

        _v[0] = axis[0]; // sin of half angle of PI is 1.0.
        _v[1] = axis[1]; // sin of half angle of PI is 1.0.
        _v[2] = axis[2]; // sin of half angle of PI is 1.0.
        _v[3] = 0; // cos of half angle of PI is zero.

    }
    else
    {
        // This is the usual situation - take a cross-product of vec1 and vec2
        // and that is the axis around which to rotate.
        Vec3d axis(from^to);
        value_type angle = acos( cosangle );
        makeRotate( angle, axis );
    }
}

//--------------------------------------------------------------------------------------------------
/// 
//--------------------------------------------------------------------------------------------------
bool StructGridCutPlane::isCellIntersectedByPlane(const Plane& plane, const Vec3d& cellMinCoord, const Vec3d& cellMaxCoord)
{
    // See http://zach.in.tu-clausthal.de/teaching/cg_literatur/lighthouse3d_view_frustum_culling/index.html

    // Start by finding the "positive vertex" and the "negative vertex" relative to plane normal
    Vec3d pVertex(cellMinCoord);
    Vec3d nVertex(cellMaxCoord);

    if (plane.A() >= 0)
    {
        pVertex.x() = cellMaxCoord.x();
        nVertex.x() = cellMinCoord.x();
    }

    if (plane.B() >= 0)
    {
        pVertex.y() = cellMaxCoord.y();
        nVertex.y() = cellMinCoord.y();
    }

    if (plane.C() >= 0)
    {
        pVertex.z() = cellMaxCoord.z();
        nVertex.z() = cellMinCoord.z();
    }

    // Chek if both positive and negative vertex are on same side of plane
    if (plane.distanceSquared(pVertex) < 0)
    {
        if (plane.distanceSquared(nVertex) < 0)
        {
            return false;
        }
        else
        {
            return true;
        }
    }
    else
    {
        if (plane.distanceSquared(nVertex) >= 0)
        {
            return false;
        }
        else
        {
            return true;
        }
    }
}

//--------------------------------------------------------------------------------------------------
/// Check if the bounding box contains the specified point
/// 
/// Note that a point on the box's surface is classified as being contained
//--------------------------------------------------------------------------------------------------
bool BoundingBox::contains(const Vec3d& point) const
{
    CVF_TIGHT_ASSERT(isValid());

    if (point.x() >= m_min.x() && point.x() <= m_max.x() &&
        point.y() >= m_min.y() && point.y() <= m_max.y() &&
        point.z() >= m_min.z() && point.z() <= m_max.z())
    {
        return true;
    }
    else
    {
        return false;
    }
}

void Octree::newNode( int depth, double x, double y, double z )
{
	double extent = boundingBox.xExtent / 2.0;

	Vec3d center;

	center.x() = boundingBox.center.x() + (extent * x);
	center.y() = boundingBox.center.y() + (extent * y);
	center.z() = boundingBox.center.z() + (extent * z);

	BoundingBox bb(center, extent, extent, extent);

	// Add child
	children.push_back(Octree());
	Octree * child = &children.back();

	child->boundingBox = bb;
	child->trianglePerNode = this->trianglePerNode;

	// Collect triangles inside child's bounding box
	for(StdVector<BaseTriangle*>::iterator it = this->triangleData.begin(); it != this->triangleData.end(); it++)
	{
		BaseTriangle* face = *it;

		if( bb.containsTriangle(face->vec(0), face->vec(1), face->vec(2)) )
		{
			child->triangleData.push_back(face);
		}
	}

	child->build(depth + 1); // build it
}

CoordinateFrame CoordinateSystemNode::computeLocalCoordinateFrame(const Vec3d& position) const
{
    if (_ellipsoidModel.valid())
    {
        Matrixd localToWorld;
        
        double latitude, longitude, height;        
        _ellipsoidModel->convertXYZToLatLongHeight(position.x(),position.y(),position.z(),latitude, longitude, height);
        _ellipsoidModel->computeLocalToWorldTransformFromLatLongHeight(latitude, longitude, 0.0f, localToWorld);

        return localToWorld;
    }
    else
    {
        return Matrixd::translate(position.x(),position.y(),0.0f);
    }
}

//--------------------------------------------------------------------------------------------------
/// 
//--------------------------------------------------------------------------------------------------
String PropertyXmlSerializer::valueTextFromVec3dVariant(const Variant& variant)
{
    CVF_ASSERT(variant.type() == Variant::VEC3D);
    Vec3d val = variant.getVec3d();

    String txt =  String::number(val.x()) + " " + String::number(val.y()) + " " + String::number(val.z());
    return txt;
}

//--------------------------------------------------------------------------------------------------
/// 
//--------------------------------------------------------------------------------------------------
Vec3d StructGridCutPlane::planeLineIntersection(const Plane& plane, const Vec3d& p1, const Vec3d& p2, const double s1, const double s2, double* s)
{
    // From http://local.wasp.uwa.edu.au/~pbourke/geometry/planeline/
    //
    // P1 (x1,y1,z1) and P2 (x2,y2,z2)
    //
    // P = P1 + u (P2 - P1)
    //
    //          A*x1 + B*y1 + C*z1 + D
    // u = ---------------------------------
    //     A*(x1-x2) + B*(y1-y2) + C*(z1-z2)

    CVF_ASSERT(s);

    const Vec3d v = p2 - p1;

    double denominator = -(plane.A()*v.x() + plane.B()*v.y() + plane.C()*v.z());
    if (denominator != 0)
    {
        double u = (plane.A()*p1.x() + plane.B()*p1.y() + plane.C()*p1.z() + plane.D())/denominator;
        if (u > 0.0 && u < 1.0)
        {
            *s = s1 + u*(s2 - s1);
            return (p1 + u*v);
        }
        else
        {
            if (u >= 1.0)
            {
                *s = s2;
                return p2;
            }
            else
            {
                *s = s1;
                return p1;
            }
        }
    }
    else
    {
        *s = s1;
        return p1;
    }
}

Vec3d scene::addVertex(Vec3d v) 
{
    v += _t;
    v = preMultd(_r, v);
    osg::Matrixd m = osg::Matrixd::translate(v.x(), v.y(), v.z());
    m = m * _m;
    Vec3d a = preMultd(m, Vec3d(0,0,0));
    _b.expandBy(a);
    return a;
}