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

Vec3d DistWeightFunc::spacedw(const Vec3d& _q) const
{
	Vec3d result(0.0, 0.0, 0.0);
	double d = _q.norm();
	if (d <= m_t && d != 0.0) // if d==0, then dw is not defined
		result = (_q / (d * m_t)) * m_wk->df(d / m_t);
	return result;
}

void glutMotion(int x, int y)
{
    if (gIsRotatingCamera)
    {
        static const double kTrackBallRadius = 0.8;   

        Vec3d lastPos;
        lastPos[0] = gLastMouseX * 2.0 / gWindowWidth - 1.0;
        lastPos[1] = (gWindowHeight - gLastMouseY) * 2.0 / gWindowHeight - 1.0;
        lastPos[2] = projectToTrackball(kTrackBallRadius, lastPos[0], lastPos[1]);

        Vec3d currPos;
        currPos[0] = x * 2.0 / gWindowWidth - 1.0;
        currPos[1] = (gWindowHeight - y) * 2.0 / gWindowHeight - 1.0;
        currPos[2] = projectToTrackball(kTrackBallRadius, currPos[0], currPos[1]);

        currPos.normalize();
        lastPos.normalize();

        Vec3d rotateVec = lastPos.cross(currPos);
				        
        double rotateAngle = asin(rotateVec.norm());
        if (fabs(rotateAngle) > 1e-6)
        {
			double deltaRotation[16];
            
			generateRotationMatrix(deltaRotation, rotateAngle, rotateVec[0], rotateVec[1], rotateVec[2]);
                
            multRight(gCameraRotation, deltaRotation);
        
            updateCamera();
        }
    }
    else if (gIsScalingCamera)
    {
        float y1 = gWindowHeight - gLastMouseY;
        float y2 = gWindowHeight - y;

        gCameraScale *= 1 + (y1 - y2) / gWindowHeight;  

        updateCamera();
    }

    gLastMouseX = x;
    gLastMouseY = y;
}

void RIMLS::Fit()
{
	double f = 0;

	Vec3d grad_f(0, 0, 0);
	do 
	{
		int i = 0;
		do 
		{
			double sumW, sumF;
			sumW = sumF = 0;
			Vec3d sumGW, sumGF, sumN;
			sumGW = sumGF = sumN = Vec3d(0.0, 0.0, 0.0);

			for (DataPoint& p : m_neighbors)
			{
				Vec3d px = m_x - p.pos();
				double fx = px.dot(p.normal());

				double alpha = 1.0;
				if (i > 0)
				{
					alpha = exp(-pow((fx - f) / m_sigmaR, 2)) *
						exp(-pow((p.normal() - grad_f).norm() / m_sigmaN, 2));
				}

				double phi = exp(-pow(px.norm() / m_sigmaT, 2));
				double w = alpha*phi;
				Vec3d grad_w = -2.0*alpha*phi*px / pow(m_sigmaT, 2);

				sumW += w;
				sumGW += grad_w;
				sumF += w*fx;
				sumGF += grad_w*fx;
				sumN += w*p.normal();
			}
			f = sumF / sumW;
			grad_f = (sumGF - f*sumGW + sumN) / sumW;
		} while (++i<m_iter && !convergence());

		m_x -= f*grad_f;

	} while ((f*grad_f).norm() > m_threshold);
}

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

double DistWeightFunc::w(const Vec3d& _q) const
{
	double d = _q.norm();
	return (d <= m_t) ? m_wk->f(d / m_t) : 0.0;
}

double DistWeightFunc::scaledw(const Vec3d& _q) const
{
	double d = _q.norm();
	return (d <= m_t) ? (-d*m_wk->df(d / m_t) / (m_t*m_t)) : 0.0;
}

std::vector<Vec3d> ClosedPolygon::getEqualDistancePoints(int numSides, const Vec3d& center)
{
	std::vector<Vec3d> result;

	int N = closedPoints.size();

	if(N < 1)	return result; // empty polygon

	for(int i = 0; i < N; i++)
		lines.push_back(Line(closedPoints[i], closedPoints[(i+1) % N], i));

	this->computeLengths();

	// Distance to walk on polygon
	double segmentLength = this->closedLength / numSides;

	// Locate start point using vecUp
	Vec3d startPoint;
	int startIndex = 0;

	Plane halfPlane(vecUp, center);
	//testPlanes1.push_back(halfPlane);

	double minDist = DBL_MAX;

	// Test intersection with all lines and remember minimum one
	for(int i = 0; i < N; i++)
	{
		Vec3d pointIntersect;

		int res = halfPlane.LineIntersect(lines[i], pointIntersect);

		if(res == INTERSECT || res == ENDPOINT_INTERSECT)
		{
			Vec3d toIntsect = pointIntersect - center;

			if(toIntsect.norm() < minDist && dot(toIntsect, vecB) > 0)
			{
				minDist = toIntsect.norm();

				startPoint = pointIntersect;
				startIndex = i;
			}
		}
	}

	double t = lines[startIndex].timeAt(startPoint);
	int index = startIndex;

	// Compute equal-dist points on polygon
	for(int s = 0; s < numSides; s++)
	{
		// Add new point
		result.push_back(lines[index].pointAt(t));

		walk(segmentLength, t, index, &t, &index);
	}

	// if polygon is opposite direction then reverse 
	if( signedArea(result, plane.n, center) < 0 )
	{
		std::reverse(result.begin(), result.end());
		std::rotate(result.begin(), result.begin()+result.size()-1 , result.end());
	}

	return closedPoints = result;
}

bool ConvexHull3::getExtremes( std::vector<int> &mExtreme )
{
	bool mExtremeCCW = false;
	mExtreme.resize(4, 0);
	int nbPnts = mPnts.size();

	// Compute the axis-aligned bounding box for the input mPnts.  Keep track
	// of the indices into 'mPnts' for the current min and max.
	Vec3d mMin, mMax;
	int j, indexMin[3], indexMax[3];
	for (j = 0; j < 3; ++j)
	{
		mMin[j] = mPnts[0][j];
		mMax[j] = mMin[j];
		indexMin[j] = 0;
		indexMax[j] = 0;
	}

	int i;
	for (i = 1; i < nbPnts; ++i)
	{
		for (j = 0; j < 3; ++j)
		{
			if (mPnts[i][j] < mMin[j])
			{
				mMin[j] = mPnts[i][j];
				indexMin[j] = i;
			}
			else if (mPnts[i][j] > mMax[j])
			{
				mMax[j] = mPnts[i][j];
				indexMax[j] = i;
			}
		}
	}

	// Determine the maximum range for the bounding box.
	Real mMaxRange = mMax[0] - mMin[0];
	mExtreme[0] = indexMin[0];
	mExtreme[1] = indexMax[0];
	Real range = mMax[1] - mMin[1];
	if (range > mMaxRange)
	{
		mMaxRange = range;
		mExtreme[0] = indexMin[1];
		mExtreme[1] = indexMax[1];
	}
	range = mMax[2] - mMin[2];
	if (range > mMaxRange)
	{
		mMaxRange = range;
		mExtreme[0] = indexMin[2];
		mExtreme[1] = indexMax[2];
	}

	// The origin is either the point of minimum x-value, point of
	// minimum y-value, or point of minimum z-value.
	Vec3d mOrigin = mPnts[mExtreme[0]];


	// Test whether the point set is (nearly) a line segment.
	std::vector<Vec3d> mDirection(3, Vec3d());
	mDirection[0] = mPnts[mExtreme[1]] - mOrigin;
	mDirection[0].normalize();
	Real maxDistance = (Real)0;
	Real distance, Dot;
	mExtreme[2] = mExtreme[0];
	for (i = 0; i < nbPnts; ++i)
	{
		Vec3d diff = mPnts[i] - mOrigin;
		Dot = dot(mDirection[0], diff);
		Vec3d proj = diff - Dot*mDirection[0];
		distance = proj.norm();
		if (distance > maxDistance)
		{
			maxDistance = distance;
			mExtreme[2] = i;
		}
	}



	// Test whether the point set is (nearly) a planar polygon.
	mDirection[1] = mPnts[mExtreme[2]] - mOrigin;
	Dot = dot(mDirection[0], mDirection[1]);
	mDirection[1] -= Dot*mDirection[0];
	mDirection[1].normalize();
	mDirection[2] = cross(mDirection[0], mDirection[1]);
	maxDistance = (Real)0;
	Real maxSign = (Real)0;
	mExtreme[3] = mExtreme[0];
	for (i = 0; i < nbPnts; ++i)
	{
		Vec3d diff = mPnts[i] - mOrigin;
		distance = dot(mDirection[2], diff);
		if (fabs(distance) > maxDistance)
		{
			maxDistance = fabs(distance);
			mExtremeCCW = (distance > (Real)0);
			mExtreme[3] = i;
//.........这里部分代码省略.........