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