自学教程：C++ ClosestPoint函数代码示例

/// [groupSyntax]float3 Triangle::ClosestPoint(const LineSegment &lineSegment, float3 *otherPt) const{    ///@todo The Triangle-LineSegment test is naive. Optimize!    float3 closestToA = ClosestPoint(lineSegment.a);    float3 closestToB = ClosestPoint(lineSegment.b);    float d;    float3 closestToSegment = ClosestPointToTriangleEdge(lineSegment, 0, 0, &d);    float3 segmentPt = lineSegment.GetPoint(d);    float distA = closestToA.DistanceSq(lineSegment.a);    float distB = closestToB.DistanceSq(lineSegment.b);    float distC = closestToSegment.DistanceSq(segmentPt);    if (distA <= distB && distA <= distC)    {        if (otherPt)            *otherPt = lineSegment.a;        return closestToA;    }    else if (distB <= distC)    {        if (otherPt)            *otherPt = lineSegment.b;        return closestToB;    }    else    {        if (otherPt)            *otherPt = segmentPt;        return closestToSegment;    }}

float3 Ray::ClosestPoint(const Ray &other, float *d, float *d2) const{	float u, u2;	float3 closestPoint = Line::ClosestPointLineLine(pos, pos + dir, other.pos, other.pos + other.dir, &u, &u2);	if (u < 0.f && u2 < 0.f)	{		closestPoint = ClosestPoint(other.pos, &u);		float3 closestPoint2 = other.ClosestPoint(pos, &u2);		if (closestPoint.DistanceSq(other.pos) <= closestPoint2.DistanceSq(pos))		{			if (d)				*d = u;			if (d2)				*d2 = 0.f;			return closestPoint;		}		else		{			if (d)				*d = 0.f;			if (d2)				*d2 = u2;			return pos;		}	}	else if (u < 0.f)	{		if (d)			*d = 0.f;		if (d2)		{			other.ClosestPoint(pos, &u2);			*d2 = Max(0.f, u2);		}		return pos;	}	else if (u2 < 0.f)	{		float3 pt = ClosestPoint(other.pos, &u);		u = Max(0.f, u);		if (d)			*d = u;		if (d2)			*d2 = 0.f;		return pt;	}	else	{		if (d)			*d = u;		if (d2)			*d2 = u2;		return closestPoint;	}}

vec Ray::ClosestPoint(const LineSegment &other, float &d, float &d2) const{	Line::ClosestPointLineLine(pos, dir, other.a, other.b - other.a, d, d2);	if (d < 0.f)	{		d = 0.f;		if (d2 >= 0.f && d2 <= 1.f)		{			other.ClosestPoint(pos, d2);			return pos;		}		vec p;		float t2;		if (d2 < 0.f)		{			p = other.a;			t2 = 0.f;		}		else // u2 > 1.f		{			p = other.b;			t2 = 1.f;		}		vec closestPoint = ClosestPoint(p, d);		vec closestPoint2 = other.ClosestPoint(pos, d2);		if (closestPoint.DistanceSq(p) <= closestPoint2.DistanceSq(pos))		{			d2 = t2;			return closestPoint;		}		else		{			d = 0.f;			return pos;		}	}	else if (d2 < 0.f)	{		d2 = 0.f;		return ClosestPoint(other.a, d);	}	else if (d2 > 1.f)	{		d2 = 1.f;		return ClosestPoint(other.b, d);	}	else		return GetPoint(d);}

void UCheatManager::TestCollisionDistance(){	APlayerController* PC = GetOuterAPlayerController();	if(PC)	{		// Get view location to act as start point		FVector ViewLoc;		FRotator ViewRot;		PC->GetPlayerViewPoint(ViewLoc, ViewRot);		FlushPersistentDebugLines( GetOuterAPlayerController()->GetWorld() );//change the GetWorld		// calculate from viewloc		for (FObjectIterator Iter(AVolume::StaticClass()); Iter; ++Iter)		{			AVolume * Volume = Cast<AVolume>(*Iter);			if (Volume->GetClass()->GetDefaultObject() != Volume)			{				FVector ClosestPoint(0,0,0);				float Distance = Volume->GetBrushComponent()->GetDistanceToCollision(ViewLoc, ClosestPoint);				float NormalizedDistance = FMath::Clamp<float>(Distance, 0.f, 1000.f)/1000.f;				FColor DrawColor(255*NormalizedDistance, 255*(1-NormalizedDistance), 0);				DrawDebugLine(GetWorld(), ViewLoc, ClosestPoint, DrawColor, true);				UE_LOG(LogCheatManager, Log, TEXT("Distance to (%s) is %0.2f"), *Volume->GetName(), Distance);			}		}	}}

std::pair<Eigen::MatrixXd, std::set<std::pair<int, int>>> UpdateGuessICP(	std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> const& reference,	std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> const& toSolve,	Eigen::MatrixXd guess){	int count = std::min(reference.size(), toSolve.size()) / 2;	std::set<int> skipRef;	std::set<int> skipSolve;	Eigen::MatrixXd refMat(2, count);	Eigen::MatrixXd solveMat(3, count);	std::set<std::pair<int, int>> matches;	for (int i = 0; i < count; i++)	{		auto closestPair = ClosestPoint(reference, toSolve, guess, skipRef, skipSolve);		if (closestPair.first == -1 || closestPair.second == -1)		{			throw std::runtime_error("Could not find enough matching star pairs");		}		skipRef.insert(closestPair.first);		skipSolve.insert(closestPair.second);		matches.insert(closestPair);		refMat.col(i) = reference[closestPair.first];		auto sVec = toSolve[closestPair.second];		solveMat.col(i) = Eigen::Vector3d(sVec[0], sVec[1], 1);	}	Eigen::MatrixXd mul = refMat * solveMat.transpose() * (solveMat * solveMat.transpose()).inverse();	if (mul.hasNaN())		throw std::runtime_error("Solved transformation had NaN");	return make_pair(mul, matches);}

float Ray::Distance(const Line &other, float *d, float *d2) const{    float u2;    float3 c = ClosestPoint(other, d, &u2);    if (d2) *d2 = u2;    return c.Distance(other.GetPoint(u2));}

float OBB::Distance(const float3 &point) const{    ////todo This code can be optimized a bit. See Christer Ericson's Real-Time Collision Detection,    /// p.134.    float3 closestPoint = ClosestPoint(point);    return point.Distance(closestPoint);}

vec Line::ClosestPoint(const LineSegment &other, float &d, float &d2) const{    ClosestPointLineLine(pos, dir, other.a, other.b - other.a, d, d2);    if (d2 < 0.f)    {        d2 = 0.f;        return ClosestPoint(other.a, d);    }    else if (d2 > 1.f)    {        d2 = 1.f;        return ClosestPoint(other.b, d);    }    else        return GetPoint(d);}

float Line::Distance(const LineSegment &other, float &d, float &d2) const{    vec c = ClosestPoint(other, d, d2);    mathassert(d2 >= 0.f);    mathassert(d2 <= 1.f);    return c.Distance(other.GetPoint(d2));}

float Line::Distance(const LineSegment &other, float *d, float *d2) const{	float u2;	vec c = ClosestPoint(other, d, &u2);	if (d2) *d2 = u2;	mathassert(u2 >= 0.f);	mathassert(u2 <= 1.f);	return c.Distance(other.GetPoint(u2));}

bool AABB::Intersects(const Sphere &sphere, vec *closestPointOnAABB) const{	// Find the point on this AABB closest to the sphere center.	vec pt = ClosestPoint(sphere.pos);	// If that point is inside sphere, the AABB and sphere intersect.	if (closestPointOnAABB)		*closestPointOnAABB = pt;	return pt.DistanceSq(sphere.pos) <= sphere.r * sphere.r;}

float3 Triangle::ClosestPoint(const LineSegment &lineSegment, float3 *otherPt) const{	////todo Optimize.	float3 intersectionPoint;	if (Intersects(lineSegment, 0, &intersectionPoint))	{		if (otherPt)			*otherPt = intersectionPoint;		return intersectionPoint;	}	float u1,v1,d1;	float3 pt1 = ClosestPointToTriangleEdge(lineSegment, &u1, &v1, &d1);	float3 pt2 = ClosestPoint(lineSegment.a);	float3 pt3 = ClosestPoint(lineSegment.b);		float D1 = pt1.DistanceSq(lineSegment.GetPoint(d1));	float D2 = pt2.DistanceSq(lineSegment.a);	float D3 = pt3.DistanceSq(lineSegment.b);	if (D1 <= D2 && D1 <= D3)	{		if (otherPt)			*otherPt = lineSegment.GetPoint(d1);		return pt1;	}	else if (D2 <= D3)	{		if (otherPt)			*otherPt = lineSegment.a;		return pt2;	}	else	{		if (otherPt)			*otherPt = lineSegment.b;		return pt3;	}}

bool APhysicsVolume::IsOverlapInVolume(const class USceneComponent& TestComponent) const{	bool bInsideVolume = true;	if (!bPhysicsOnContact)	{		FVector ClosestPoint(0.f);		UPrimitiveComponent* RootPrimitive = Cast<UPrimitiveComponent>(GetRootComponent());		const float DistToCollision = RootPrimitive ? RootPrimitive->GetDistanceToCollision(TestComponent.GetComponentLocation(), ClosestPoint) : 0.f;		bInsideVolume = (DistToCollision == 0.f);	}	return bInsideVolume;}

vec Line::ClosestPoint(const LineSegment &other, float *d, float *d2) const{	float t2;	vec closestPoint = ClosestPointLineLine(pos, pos + dir, other.a, other.b, d, &t2);	if (t2 <= 0.f)	{		if (d2)			*d2 = 0.f;		return ClosestPoint(other.a, d);	}	else if (t2 >= 1.f)	{		if (d2)			*d2 = 1.f;		return ClosestPoint(other.b, d);	}	else	{		if (d2)			*d2 = t2;		return closestPoint;	}}

Manipulator* MultiLineView::CreateManipulator (    Viewer* v, Event& e, Transformer* rel, Tool* tool) {    Manipulator* m = nil;    if (tool->IsA(GRAPHIC_COMP_TOOL)) {        v->Constrain(e.x, e.y);        Coord x[1], y[1];        x[0] = e.x;        y[0] = e.y;        GrowingVertices* rub = new GrowingMultiLine(            nil, nil, x, y, 1, -1, HANDLE_SIZE        );        m = new VertexManip(	    v, rub, rel, tool, DragConstraint(HorizOrVert | Gravity)	);    } else if (tool->IsA(RESHAPE_TOOL)) {	Coord* x, *y;	int n;        v->Constrain(e.x, e.y);	GetVertices(x, y, n);        GrowingMultiLine* rub = new GrowingMultiLine(            nil, nil, x, y, n, ClosestPoint(x, y, n, e.x, e.y), HANDLE_SIZE        );	delete x;	delete y;        m = new VertexManip(	    v, rub, rel, tool, DragConstraint(HorizOrVert | Gravity)	);    } else {        m = VerticesView::CreateManipulator(v, e, rel, tool);    }    return m;}

float Polygon::Distance(const float3 &point) const{	float3 pt = ClosestPoint(point);	return pt.Distance(point);}

cv::Vec3b Camera::CastRay(Scene scene, Ray r){  std::vector<SpherePoint> Intersections =     FindIntersectionPoints(scene.GetSphereList(), r);  cv::Vec3b ReturnColor(255, 255, 255);  // If the ray intersected with any of them, find the closest point  if (Intersections.size() != 0)  {    Sphere ClosestSphere(Point(0,0,0),0,cv::Vec3b(0,0,0),Finish(0,0,0,0));    Point ClosestPoint(0, 0, 0);    double ShortestDistance = std::numeric_limits<double>::max();    for (auto& SpherePoint : Intersections)    {      Sphere& S = SpherePoint.s;      Point& P = SpherePoint.p;      double TempDist = r.Location.FromThisToThat(P).Length();      if (TempDist < ShortestDistance)      {        ShortestDistance = TempDist;        ClosestSphere = S;        ClosestPoint = P;      }    }    // Compute ambient component    cv::Vec3d& SceneAmb = scene.GetAmbientLightColor();    cv::Vec3b SphereAmbientColor = ClosestSphere.Color * ClosestSphere.Fin.Ambient;    SphereAmbientColor[0] *= SceneAmb[0];    SphereAmbientColor[1] *= SceneAmb[1];    SphereAmbientColor[2] *= SceneAmb[2];    // Compute diffuse component    // Translate the intersection point to avoid precision errors    cv::Vec3b DiffuseColor(0, 0, 0);    cv::Vec3b SpecularColor(0, 0, 0);    Vector SphereNormal = SphereNormalAtPoint(ClosestSphere, ClosestPoint);    ClosestPoint = ClosestPoint.Translate(SphereNormal * 0.01);    Vector PointToLight = ClosestPoint.FromThisToThat(scene.GetLightPosition()).Normalize();    // Check to see if intersection point is on the same side as the light    // And to make sure there is not another sphere in the way    double DotProduct = PointToLight.Dot(SphereNormal);    if (DotProduct > 0)    {      Intersections = FindIntersectionPoints(scene.GetSphereList(), Ray(ClosestPoint, PointToLight));      // Need to account for when spheres are past the light      if (Intersections.size() == 0)      {        cv::Vec3d& LightColor = scene.GetLightColor();        DiffuseColor = ClosestSphere.Color * ClosestSphere.Fin.Diffuse;        DiffuseColor[0] *= DotProduct * LightColor[0];        DiffuseColor[1] *= DotProduct * LightColor[1];        DiffuseColor[2] *= DotProduct * LightColor[2];        // Compute the specular light contribution        Vector ReflectionVector = PointToLight - (SphereNormal * (2 * DotProduct));        Vector ViewDirection = r.Location.FromThisToThat(ClosestPoint).Normalize();        double SpecIntensity = ReflectionVector.Dot(ViewDirection);        if (SpecIntensity > 0)        {          double Multiplier =             ClosestSphere.Fin.Specular *            std::pow(SpecIntensity, 1 / ClosestSphere.Fin.Roughness);          SpecularColor[0] = Multiplier * LightColor[0] * 255;          SpecularColor[1] = Multiplier * LightColor[1] * 255;          SpecularColor[2] = Multiplier * LightColor[2] * 255;        }      }    }    ReturnColor = SphereAmbientColor + DiffuseColor + SpecularColor;  }  return ReturnColor;}

float Frustum::Distance(const float3 &point) const{	float3 pt = ClosestPoint(point);	return pt.Distance(point);}

Manipulator* RectView::CreateManipulator (    Viewer* v, Event& e, Transformer* rel, Tool* tool) {    Coord x[5], y[5];    Rubberband* rub = nil;    Manipulator* m = nil;    if (tool->IsA(GRAPHIC_COMP_TOOL)) {        v->Constrain(e.x, e.y);        rub = new RubberRect(nil, nil, e.x, e.y, e.x, e.y);        m = new DragManip(	    v, rub, rel, tool, DragConstraint(XYEqual | Gravity)	);    } else if (tool->IsA(RESHAPE_TOOL)) {	RubberGroup* rub = new RubberGroup(nil, nil);	Coord x[4], y[4];        v->Constrain(e.x, e.y);	GetCorners(x, y);	_reshapeCorner = ClosestPoint(x, y, 4, e.x, e.y);	if (_reshapeCorner > 0) {	    rub->Append(		new RubberLine(                    nil, nil, x[_reshapeCorner-1], y[_reshapeCorner-1], e.x,e.y                )	    );	} else { 	    rub->Append(new RubberLine(nil,nil,x[3],y[3],e.x,e.y));	}	if (_reshapeCorner < 3) {	    rub->Append(		new RubberLine(                    nil, nil, x[_reshapeCorner+1], y[_reshapeCorner+1], e.x,e.y                )	    );	} else { 	    rub->Append(new RubberLine(nil, nil, x[0], y[0], e.x, e.y));	}        m = new DragManip(	    v, rub, rel, tool, DragConstraint(HorizOrVert | Gravity)	);    } else if (tool->IsA(MOVE_TOOL)) {        v->Constrain(e.x, e.y);        GetCorners(x, y);        x[4] = x[0]; y[4] = y[0];	rub = new SlidingLineList(nil, nil, x, y, 5, e.x, e.y);        m = new DragManip(	    v, rub, rel, tool, DragConstraint(HorizOrVert | Gravity)	);    } else if (tool->IsA(SCALE_TOOL)) {        v->Constrain(e.x, e.y);        GetCorners(x, y);        x[4] = x[0]; y[4] = y[0];        rub = new ScalingLineList(nil,nil,x,y,5, (x[0]+x[2])/2, (y[0]+y[2])/2);        m = new DragManip(v, rub, rel, tool, Gravity);    } else if (tool->IsA(ROTATE_TOOL)) {        v->Constrain(e.x, e.y);        GetCorners(x, y);        x[4] = x[0]; y[4] = y[0];        rub = new RotatingLineList(            nil, nil, x, y, 5, (x[0]+x[2])/2, (y[0]+y[2])/2, e.x, e.y        );        m = new DragManip(v, rub, rel, tool, Gravity);    } else {        m = GraphicView::CreateManipulator(v, e, rel, tool);    }    return m;}