/// [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));
}
/** For Triangle-Sphere intersection code, see Christer Ericson's Real-Time Collision Detection, p.167. */
bool Triangle::Intersects(const Sphere &sphere, float3 *closestPointOnTriangle) const
{
float3 pt = ClosestPoint(sphere.pos);
if (closestPointOnTriangle)
*closestPointOnTriangle = pt;
return pt.DistanceSq(sphere.pos) <= sphere.r * sphere.r;
}
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;
}
vec Line::ClosestPoint(const Ray &other, float &d, float &d2) const
{
ClosestPointLineLine(pos, dir, other.pos, other.dir, d, d2);
if (d2 >= 0.f)
return GetPoint(d);
else
{
d2 = 0.f;
return ClosestPoint(other.pos, d);
}
}
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;
}
}
vec Line::ClosestPoint(const Triangle &triangle, float *outU, float *outV, float *outD) const
{
///\todo Optimize this function!
vec closestPointTriangle = triangle.ClosestPoint(*this);
if (outU || outV)
{
float2 uv = triangle.BarycentricUV(closestPointTriangle);
if (outU)
*outU = uv.x;
if (outV)
*outV = uv.y;
}
return ClosestPoint(closestPointTriangle, outD);
}
vec Ray::ClosestPoint(const Ray &other, float &d, float &d2) const
{
Line::ClosestPointLineLine(pos, dir, other.pos, other.dir, d, d2);
if (d < 0.f && d2 < 0.f)
{
vec closestPoint = ClosestPoint(other.pos, d);
vec closestPoint2 = other.ClosestPoint(pos, d2);
if (closestPoint.DistanceSq(other.pos) <= closestPoint2.DistanceSq(pos))
{
d2 = 0.f;
return closestPoint;
}
else
{
d = 0.f;
return pos;
}
}
else if (d < 0.f)
{
d = 0.f;
other.ClosestPoint(pos, d2);
d2 = Max(0.f, d2);
return pos;
}
else if (d2 < 0.f)
{
vec pt = ClosestPoint(other.pos, d);
d = Max(0.f, d);
d2 = 0.f;
return pt;
}
else
{
return GetPoint(d);
}
}
vec Line::ClosestPoint(const Ray &other, float *d, float *d2) const
{
float t2;
vec closestPoint = ClosestPointLineLine(pos, pos + dir, other.pos, other.pos + other.dir, d, &t2);
if (t2 <= 0.f)
{
if (d2)
*d2 = 0.f;
return ClosestPoint(other.pos, 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);
}
float3 Polygon::ClosestPoint(const LineSegment &lineSegment) const
{
return ClosestPoint(lineSegment, 0);
}
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 Ray::Distance(const vec &point, float &d) const
{
return ClosestPoint(point, d).Distance(point);
}
float AABB::Distance(const vec &point) const
{
///@todo This function could be slightly optimized. See Christer Ericson's
/// Real-Time Collision Detection, p.131.
return ClosestPoint(point).Distance(point);
}
float Ray::Distance(const LineSegment &other, float &d, float &d2) const
{
vec c = ClosestPoint(other, d, d2);
return c.Distance(other.GetPoint(d2));
}
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;
}
bool Ray::Contains(const float3 &point, float distanceThreshold) const
{
return ClosestPoint(point).DistanceSq(point) <= distanceThreshold;
}
/// Returns the distance of the given point to this line.
/// @param d [out] This element will receive the distance along this line that specifies the closest point on this line to the given point.
float Ray::Distance(const float3 &point, float *d) const
{
return ClosestPoint(point, d).Distance(point);
}
