//test for line-of-sight - face intersection is hard, so cheat slightly
//and look along the y=0 base plane for a line intersection.
//Returns index of first object hit.
int LineHitsObject(Point3d gunPoint, Point3d gunAngle) {
int i, j, ret;
float nearest, distance;
Point3d start, end;
ret=0;
nearest=0.0;
//Cycle through objects
for (i=1; i<=world.numObjects; i++) {
//cycle through edges
for (j=0; j < world.objects[i].numEdges;j++) {
//transform with respect to the gun position and orientation
start=TransformPoint(world.objects[i].points[world.objects[i].edges[j].start], gunPoint, gunAngle);
end=TransformPoint(world.objects[i].points[world.objects[i].edges[j].end], gunPoint, gunAngle);
//ignore height - if the edge's start and end points are left and right of the gun, and in front, it's a hit
if (((start.x <=0 && end.x >= 0) || (start.x >=0 && end.x <= 0)) && start.z >0 && end.z > 0) {
//Hit! Is it nearer than the last one?
distance = start.z;
if (start.x != end.x)
distance += start.x * (end.z - start.z) / (end.x - start.x);
if (ret==0 || distance < nearest) {
ret=i;
nearest = distance;
break; //jump out of for loop (for j)
}
}
}
}
return ret; //will be 0 if none found - our objects start at 1 anyway
}
void CGraph::DrawPoint()
{
EnterCriticalSection(&m_cs);
if(m_iGraphPointCurrentNum != 0)
{
m_hOldDrawPointPen = (HPEN)::SelectObject(m_hMemDC, m_hDrawPointPen);
//计算比例
for(std::deque<GraphDataInfoStruct>::size_type i = 0; i < m_iGraphPointCurrentNum; ++i)
{
GraphDataInfoStruct gd = m_GraphDataInfoStructDeque[i];
double gdPoint[CNC_AXIS_NUM];
memcpy(gdPoint, gd.nAxisRealPos, sizeof(double) * CNC_AXIS_NUM);
RefreshPointMinAndMax(gdPoint);
double xRate = (m_GraphPointMaxX - m_GraphPointMinX) / m_cxExcludeFrameStaticWidth;
double yRate = (m_GraphPointMaxY - m_GraphPointMinY) / m_cyExcludeFrameStaticHeight;
//fTempRate = xRate >= yRate ? xRate : yRate;
//g_fRate = g_fRate > fTempRate ? g_fRate : fTempRate;
g_fRate = xRate >= yRate ? xRate : yRate;
}
for(std::deque<GraphDataInfoStruct>::size_type i = 0; i < m_iGraphPointCurrentNum - 1; ++i)
{
GraphDataInfoStruct gd1 = m_GraphDataInfoStructDeque[i];
GraphDataInfoStruct gd2 = m_GraphDataInfoStructDeque[i + 1];
double BeforeTransformGd1Point[CNC_AXIS_NUM], BeforeTransformGd2Point[CNC_AXIS_NUM];
memcpy(BeforeTransformGd1Point, gd1.nAxisRealPos, sizeof(double) * CNC_AXIS_NUM);
memcpy(BeforeTransformGd2Point, gd2.nAxisRealPos, sizeof(double) * CNC_AXIS_NUM);
////获取区域大小
//RefreshPointMinAndMax(BeforeTransformGd1Point);
//RefreshPointMinAndMax(BeforeTransformGd2Point);
double *AfterTransformGd1Point = TransformPoint(BeforeTransformGd1Point);
double *AfterTransformGd2Point = TransformPoint(BeforeTransformGd2Point);
//为防止像素点为同一点，过滤
if(fabs(int(AfterTransformGd2Point[0]) * 1.0 - int(AfterTransformGd1Point[0]) * 1.0) >= 1.0 ||
fabs(int(AfterTransformGd2Point[1]) * 1.0 - int(AfterTransformGd1Point[1]) * 1.0) >= 1.0)
{
::MoveToEx(m_hMemDC, AfterTransformGd1Point[0], AfterTransformGd1Point[1], NULL);
::LineTo(m_hMemDC, AfterTransformGd2Point[0], AfterTransformGd2Point[1]);
}
delete[] AfterTransformGd1Point;
delete[] AfterTransformGd2Point;
}
::SelectObject(m_hMemDC, m_hOldDrawPointPen);
double BeforeTransformLastGd[CNC_AXIS_NUM];
memcpy(BeforeTransformLastGd, m_GraphDataInfoStructDeque[m_iGraphPointCurrentNum - 1].nAxisRealPos, sizeof(double) * CNC_AXIS_NUM);
double* AfterTransformLastGd = TransformPoint(BeforeTransformLastGd);
DrawToolPoint(AfterTransformLastGd[0], AfterTransformLastGd[1]);
}
LeaveCriticalSection(&m_cs);
}
FloatRect Transform::TransformRect(const FloatRect& rectangle) const
{
// Transform the 4 corners of the rectangle
const Vector2f points[] =
{
TransformPoint(rectangle.Left, rectangle.Top),
TransformPoint(rectangle.Left, rectangle.Top + rectangle.Height),
TransformPoint(rectangle.Left + rectangle.Width, rectangle.Top),
TransformPoint(rectangle.Left + rectangle.Width, rectangle.Top + rectangle.Height)
};
// Compute the bounding rectangle of the transformed points
float left = points[0].x;
float top = points[0].y;
float right = points[0].x;
float bottom = points[0].y;
for (int i = 1; i < 4; ++i)
{
if (points[i].x < left) left = points[i].x;
else if (points[i].x > right) right = points[i].x;
if (points[i].y < top) top = points[i].y;
else if (points[i].y > bottom) bottom = points[i].y;
}
return FloatRect(left, top, right - left, bottom - top);
}
static std::pair<Float2, Float2> GetNodeInCell(const TerrainCell& cell, unsigned nodeIndex)
{
const auto& sourceNode = *cell._nodes[nodeIndex];
auto nodeMinInCell = Truncate(TransformPoint(sourceNode._localToCell, Float3(0.f, 0.f, 0.f)));
auto nodeMaxInCell = Truncate(TransformPoint(sourceNode._localToCell, Float3(1.f, 1.f, 1.f)));
return std::make_pair(nodeMinInCell, nodeMaxInCell);
}
static void ddRenderVecs(const mat4& projview)
{
const ShaderInfo* shader = g_dbgdrawShader.get();
GLint posLoc = shader->m_attrs[GEOM_Pos];
GLint colorLoc = shader->m_attrs[GEOM_Color];
GLint mvpLoc = shader->m_uniforms[BIND_Mvp];
glUniformMatrix4fv(mvpLoc, 1, 0, projview.m);
const ddVec* cur = g_lists.m_vecs;
if(cur)
{
glBegin(GL_LINES);
while(cur)
{
vec3 from = TransformPoint(cur->m_xfm, cur->m_from);
vec3 to = TransformPoint(cur->m_xfm, cur->m_from + cur->m_vec);
glVertexAttrib3fv(colorLoc, &cur->m_color.r);
glVertexAttrib3fv(posLoc, &from.x);
glVertexAttrib3fv(posLoc, &to.x);
cur = cur->m_next;
}
glEnd();
}
}
float SSVTreeCollider::LssPssDist(float r0, const Point& point0, const Point& point1, float r1, const Point& center0)
{
Point p0, p1;
TransformPoint(p0, point0, mR0to1, mT0to1);
TransformPoint(p1, point1, mR0to1, mT0to1);
float d = PointSegDist(center0, p0, p1);
return d - r0 - r1;
}
/** Draws the eyebrow onto Head Top
* \param p1 Point to start
* \param p2 Point to finish
* \param graphics Graphics context to draw on
*/
void CHeadTop::DrawEyebrow(Point p1, Point p2, Gdiplus::Graphics *graphics)
{
Gdiplus::Point start = TransformPoint(p1);
Gdiplus::Point finish = TransformPoint(p2);
Pen eyebrowPen(Color::Black, 2);
graphics->DrawLine(&eyebrowPen, start, finish);
}
float SSVTreeCollider::LssLssDist(float r0, const Point& point0, const Point& point1, float r1, const Point& point2, const Point& point3)
{
Point p0, p1;
TransformPoint(p0, point0, mR0to1, mT0to1);
TransformPoint(p1, point1, mR0to1, mT0to1);
float d = SegSegDist(p0, p1, point2, point3);
return d - r0 - r1;
}
bool RayVsAABB(const std::pair<Float3, Float3>& worldSpaceRay, const Float4x4& aabbToWorld, const Float3& mins, const Float3& maxs)
{
// Does this ray intersect the aabb?
// transform the ray back into aabb space, and do tests against the edge planes of the bounding box
auto worldToAabb = Inverse(aabbToWorld); // maybe not be orthonormal input. This is used for terrain, which has scale on aabbToWorld
auto ray = std::make_pair(
TransformPoint(worldToAabb, worldSpaceRay.first),
TransformPoint(worldToAabb, worldSpaceRay.second));
return RayVsAABB(ray, mins, maxs);
}
EDMath::Sphere SphereCollider::GetWorldSphere(void)
{
Sphere s;
s.radius = sphere.radius;
TransformPoint( s.center, sphere.center, GetGameObject()->GetTransform()->GetWorldMatrix() );
return s;
}
/**************************************************************************//**
* @author Caitlin Taggart
*
* @par Description:
* Generates the fractal by solving the equations and goes through the first
* couple transformed points.
*****************************************************************************/
void GenWindow::GenerateFractal(vector<vector<double> > transforms, vector<Point> p, double c[])
{
//get the transfroms
SolveEquations(transforms);
//copy the data needed
points = p;
for (int i = 0; i < 3; i++)
{
color[i] = c[i];
}
//initialize the transforms
idle.x = 0;
idle.y = 0;
//generate the first few tranformed points
for (int i = 0; i < 16; i++)
{
int random = rand() % Transforms.size();
idle = TransformPoint(idle, Transforms[random]);
}
//let the idle function know it can generate
generate = true;
//tell glut that it should redisplay points
glutSetWindow(_windowID);
glutPostRedisplay();
}
/**************************************************************************//**
* @author Caitlin Taggart
*
* @par Description:
* This is the idle call back function. This generates the points that create
* the fractal and then calls redisplay. Does this ten points at a time. Once
* this function is called and worked on for more than 100000 points it stops.
*****************************************************************************/
void GenWindow::Idle()
{
static int j = 0;
if (generate && j < 100000)
{
for (int i = 0; i < 10; i++)
{
//find a random transform
int random = rand() % Transforms.size();
//transform the last point
idle = TransformPoint(idle, Transforms[random]);
//add a color based on the transform
idle.r = Transforms[random].red;
idle.g = Transforms[random].green;
idle.b = Transforms[random].blue;
//add it to the generated points list
genPoints.push_back(idle);
j++;
}
//ask glut to redisplay
glutSetWindow(_windowID);
glutPostRedisplay();
}
}
void mlMatrix3x4::TransformPoints(mlVector3D *out, const mlVector3D *in, int numPt) const
{
for (int i = 0; i < numPt; i++)
{
*out++ = TransformPoint(*in++);
}
}
void JRTMesh::Transform(const Matrix4f& rXForm, const Matrix4f& rInverse)
{
// transform positions
for (UINT i = 0; i < m_nVertexCount; i++)
{
TransformPoint(&m_Positions[i], &rXForm, &m_Positions[i]);
}
Matrix4f inverseTranspose = rInverse.Transpose();
// transform normals
if (! m_Normals.empty ())
{
for (UINT i = 0; i < m_nVertexCount; i++)
{
ALIGN16 Vec3f tmp = m_Normals[i];
TransformVector(&tmp, &inverseTranspose, &m_Normals[i]);
m_Normals[i] = Normalize(m_Normals[i]);
}
}
else
{
// transform face normals
for (UINT i = 0; i < m_nTriangleCount; i++)
{
ALIGN16 Vec3f tmp = m_FaceNormals[i];
TransformVector(&tmp, &inverseTranspose, &m_FaceNormals[i]);
m_FaceNormals[i] = tmp;
}
}
}
bool
Image::InsideObject (cairo_t *cr, double x, double y)
{
if (!FrameworkElement::InsideObject (cr, x, y))
return false;
cairo_save (cr);
cairo_new_path (cr);
cairo_set_matrix (cr, &absolute_xform);
double nx = x;
double ny = y;
TransformPoint (&nx, &ny);
Render (cr, NULL, true);
bool inside = cairo_in_fill (cr, nx, ny);
cairo_restore (cr);
if (inside)
inside = InsideLayoutClip (x, y);
if (inside)
inside = InsideClip (cr, x, y);
return inside;
}
std::pair<Float3, Float3> TransformBoundingBox(const Float3x4& transformation, std::pair<Float3, Float3> boundingBox)
{
Float3 corners[] =
{
Float3( boundingBox.first[0], boundingBox.first[1], boundingBox.first[2] ),
Float3( boundingBox.second[0], boundingBox.first[1], boundingBox.first[2] ),
Float3( boundingBox.first[0], boundingBox.second[1], boundingBox.first[2] ),
Float3( boundingBox.second[0], boundingBox.second[1], boundingBox.first[2] ),
Float3( boundingBox.first[0], boundingBox.first[1], boundingBox.second[2] ),
Float3( boundingBox.second[0], boundingBox.first[1], boundingBox.second[2] ),
Float3( boundingBox.first[0], boundingBox.second[1], boundingBox.second[2] ),
Float3( boundingBox.second[0], boundingBox.second[1], boundingBox.second[2] )
};
for (unsigned c=0; c<dimof(corners); ++c) {
corners[c] = TransformPoint(transformation, corners[c]);
}
Float3 mins(FLT_MAX, FLT_MAX, FLT_MAX), maxs(-FLT_MAX, -FLT_MAX, -FLT_MAX);
for (unsigned c=0; c<dimof(corners); ++c) {
mins[0] = std::min(mins[0], corners[c][0]);
mins[1] = std::min(mins[1], corners[c][1]);
mins[2] = std::min(mins[2], corners[c][2]);
maxs[0] = std::max(maxs[0], corners[c][0]);
maxs[1] = std::max(maxs[1], corners[c][1]);
maxs[2] = std::max(maxs[2], corners[c][2]);
}
return std::make_pair(mins, maxs);
}
void G2API_AddSkinGore(CGhoul2Info_v &ghoul2,SSkinGoreData &gore)
{
if (VectorLength(gore.rayDirection)<.1f)
{
assert(0); // can't add gore without a shot direction
return;
}
// make sure we have transformed the whole skeletons for each model
G2_ConstructGhoulSkeleton(ghoul2, gore.currentTime, NULL, true, gore.angles, gore.position, gore.scale, false);
// pre generate the world matrix - used to transform the incoming ray
G2_GenerateWorldMatrix(gore.angles, gore.position);
// first up, translate the ray to model space
vec3_t transRayDirection, transHitLocation;
TransformAndTranslatePoint(gore.hitLocation, transHitLocation, &worldMatrixInv);
TransformPoint(gore.rayDirection, transRayDirection, &worldMatrixInv);
int lod;
ResetGoreTag();
for (lod=0;lod<4;lod++)
{
// now having done that, time to build the model
// FIXME: where does G2VertSpaceServer come from?
// G2_TransformModel(ghoul2, gore.currentTime, gore.scale,G2VertSpaceServer, lod,true);
// now walk each model and compute new texture coordinates
G2_TraceModels(ghoul2, transHitLocation, transRayDirection, 0, gore.entNum, 0, lod);
}
}
static bool FindWidgetsUnderPoint(const FVector2D& InHitTestPoint, const FVector2D& InWindowPosition, const TSharedRef<FWidgetReflectorNodeBase>& InWidget, TArray<TSharedRef<FWidgetReflectorNodeBase>>& OutWidgets)
{
const bool bNeedsHitTesting = InWidget->GetHitTestInfo().IsHitTestVisible || InWidget->GetHitTestInfo().AreChildrenHitTestVisible;
if (bNeedsHitTesting)
{
const FSlateRect HitTestRect = FSlateRect::FromPointAndExtent(
InWidget->GetAccumulatedLayoutTransform().GetTranslation() - InWindowPosition,
TransformPoint(InWidget->GetAccumulatedLayoutTransform().GetScale(), InWidget->GetLocalSize())
);
if (HitTestRect.ContainsPoint(InHitTestPoint))
{
OutWidgets.Add(InWidget);
if (InWidget->GetHitTestInfo().AreChildrenHitTestVisible)
{
for (const auto& ChildWidget : InWidget->GetChildNodes())
{
if (FindWidgetsUnderPoint(InHitTestPoint, InWindowPosition, ChildWidget, OutWidgets))
{
return true;
}
}
}
return InWidget->GetHitTestInfo().IsHitTestVisible;
}
}
return false;
}
FVector2D FSlateDrawElement::GetRotationPoint(const FPaintGeometry& PaintGeometry, const TOptional<FVector2D>& UserRotationPoint, ERotationSpace RotationSpace)
{
FVector2D RotationPoint(0, 0);
const FVector2D& LocalSize = PaintGeometry.GetLocalSize();
switch (RotationSpace)
{
case RelativeToElement:
{
// If the user did not specify a rotation point, we rotate about the center of the element
RotationPoint = UserRotationPoint.Get(LocalSize * 0.5f);
}
break;
case RelativeToWorld:
{
// its in world space, must convert the point to local space.
RotationPoint = TransformPoint(Inverse(PaintGeometry.GetAccumulatedRenderTransform()), UserRotationPoint.Get(FVector2D::ZeroVector));
}
break;
default:
check(0);
break;
}
return RotationPoint;
}
float SSVTreeCollider::PssLssDist(float r0, const Point& center0, float r1, const Point& point0, const Point& point1)
{
Point c0;
TransformPoint(c0, center0, mR0to1, mT0to1);
float d = PointSegDist(c0, point0, point1);
return d - r0 - r1;
}
liblas::Point const& ReaderImpl::ReadPointAt(std::size_t n)
{
if (m_size == n) {
throw std::out_of_range("file has no more points to read, end of file reached");
} else if (m_size < n) {
std::ostringstream msg;
msg << "ReadPointAt:: Inputted value: " << n << " is greater than the number of points: " << m_size;
throw std::runtime_error(msg.str());
}
std::streamsize const pos = (static_cast<std::streamsize>(n) * m_header->GetDataRecordLength()) + m_header->GetDataOffset();
m_ifs.clear();
m_ifs.seekg(pos, std::ios::beg);
if (bNeedHeaderCheck)
{
if (!(m_point->GetHeader().get() == *m_header))
m_point->SetHeader(HeaderOptionalConstRef(*m_header));
}
detail::read_n(m_point->GetData().front(), m_ifs, m_record_size);
if (!m_transforms.empty())
{
TransformPoint(*m_point);
}
return *m_point;
}
Ray PinholeCamera::GenerateRay(const Vector2ui &pixel, const Vector2f &sample) const {
// Compute point on view plane
Vector3f s = Vector3f(left + (right - left) * (pixel.x() + sample.x()) / (float) width,
bottom + (top - bottom) * (pixel.y() + sample.y()) / (float) height,
-1.f);
return Ray(eye_world, TransformPoint(look_at, s) - eye_world);
}
void PreviewWindow::DrawTrajectory(wxDC& dc)
{
Point previous;
dc.SetPen(wxPen(trajectoryColour, lineWidth*2, wxSOLID));
for_each(trajectory.begin(), trajectory.end(), [&](Point& p)
{
Point start = TransformPoint(previous);
Point end = TransformPoint(p);
if(drawTrajectory)
dc.DrawLine(start.x, start.y, end.x, end.y);
previous = p;
});
dc.SetPen(wxPen(trajectoryColour, lineWidth, wxSOLID));
dc.SetBrush(*wxRED_BRUSH);
previous = TransformPoint(previous);
dc.DrawCircle(previous.x, previous.y, 4);
}
void CCharShape::BuildShadowVertex (double x, double y, double z, TMatrix mat, TVector3* pt, TVector3* nml) {
double old_y;
*pt = MakeVector (x, y, z);
*pt = TransformPoint (mat, *pt);
old_y = pt->y;
*nml = Course.FindCourseNormal (pt->x, pt->z);
pt->y = Course.FindYCoord (pt->x, pt->z) + SHADOW_HEIGHT;
if (pt->y > old_y) pt->y = old_y;
}
OBB OBBTransform(const mat4& mat, const OBB& obb)
{
OBB result;
mat4 matIT = TransposeOfInverse(mat);
result.m_center = TransformPoint(mat, obb.m_center);
for(int i = 0; i < 3; ++i)
result.m_b[i] = TransformVec(matIT, obb.m_b[i]);
return result;
}
void CCharShape::DrawShadowVertex (double x, double y, double z, const TMatrix mat) {
TVector3 pt(x, y, z);
pt = TransformPoint (mat, pt);
double old_y = pt.y;
TVector3 nml = Course.FindCourseNormal (pt.x, pt.z);
pt.y = Course.FindYCoord (pt.x, pt.z) + SHADOW_HEIGHT;
if (pt.y > old_y) pt.y = old_y;
glNormal3f (nml.x, nml.y, nml.z);
glVertex3f (pt.x, pt.y, pt.z);
}
void CCharShape::DrawShadowVertex(int& n, const TMatrix<4, 4>& mat) {
TVector3d pt(vectors[n],vectors[n+1] , vectors[n+2]);
n += 3;
pt = TransformPoint (mat, pt);
ETR_DOUBLE old_y = pt.y;
//TVector3d nml = Course.FindCourseNormal (pt.x, pt.z);
pt.y = Course.FindYCoord (pt.x, pt.z) + SHADOW_HEIGHT;
if (pt.y > old_y) pt.y = old_y;
//glNormal3(nml);
glVertex3(pt);
}
void GoalCallback(const geometry_msgs::PoseStampedConstPtr &msg)
{
int i, j, k;
double roll, pitch, yaw, tfyaw;
tf::Vector3 tfpoint = TransformPoint(msg->pose);
gx = (int)(tfpoint.getX()*10);
gy = (int)(tfpoint.getY()*10);
dgx = msg->pose.position.x;
dgy = msg->pose.position.y;
tf::Quaternion quat(msg->pose.orientation.x, msg->pose.orientation.y,
msg->pose.orientation.z, msg->pose.orientation.w);
tf::Matrix3x3 m(quat);
m.getRPY(roll, pitch, yaw);
dgtheta = yaw; // in real world
tf::Quaternion tfquat = _transform * quat;
tf::Matrix3x3 tfm(tfquat);
tfm.getRPY(roll, pitch, tfyaw);
if (tfyaw < 0) {tfyaw+=2*M_PI;}
gtheta = (int)(tfyaw/THETA); // in grid cell
printf("\n(dgx, dgy, dgtheta) = (%lf, %lf, %lf)\n", dgx, dgy, dgtheta);
printf("(gx, gy, gtheta) = (%d, %d, %d)\n", gx, gy, gtheta);
/////////for debug/////////
//gx = 96; gy = 25;
//dgx = 9.62; dgy = 2.557;
//dgtheta = 1.512;
//gtheta = (int)(dgtheta/THETA);
///////////////////////////
/* goal range is about 1m square */
for (i = gy-5; i < gy+5; i++){
for (j = gx-5; j < gx+5; j++){
for (k = 0; k < SIZE_T; k++){
map[i][j][k].goal = true;
}
}
}
if (Collide(tfpoint.getX(), tfpoint.getY(), tfyaw)){
std::cout << "INVALID GOAL POINT!" << std::endl;
return;
}
/* Set WaveFront heurisic */
WaveFrontInit();
_goal_set = true;
}
void FillPolygonAreas(grid_map::GridMap &out_grid_map, const std::vector<std::vector<geometry_msgs::Point>> &in_area_points,
const std::string &in_grid_layer_name, const int in_layer_background_value,
const int in_layer_min_value, const int in_fill_color, const int in_layer_max_value,
const std::string &in_tf_target_frame, const std::string &in_tf_source_frame,
const tf::TransformListener &in_tf_listener)
{
if(!out_grid_map.exists(in_grid_layer_name))
{
out_grid_map.add(in_grid_layer_name);
}
out_grid_map[in_grid_layer_name].setConstant(in_layer_background_value);
cv::Mat original_image;
grid_map::GridMapCvConverter::toImage<unsigned char, 1>(out_grid_map,
in_grid_layer_name,
CV_8UC1,
in_layer_min_value,
in_layer_max_value,
original_image);
cv::Mat filled_image = original_image.clone();
tf::StampedTransform tf = FindTransform(in_tf_target_frame, in_tf_source_frame, in_tf_listener);
// calculate out_grid_map position
grid_map::Position map_pos = out_grid_map.getPosition();
double origin_x_offset = out_grid_map.getLength().x() / 2.0 - map_pos.x();
double origin_y_offset = out_grid_map.getLength().y() / 2.0 - map_pos.y();
for (const auto &points : in_area_points)
{
std::vector<cv::Point> cv_points;
for (const auto &p : points)
{
// transform to GridMap coordinate
geometry_msgs::Point tf_point = TransformPoint(p, tf);
// coordinate conversion for cv image
double cv_x = (out_grid_map.getLength().y() - origin_y_offset - tf_point.y) / out_grid_map.getResolution();
double cv_y = (out_grid_map.getLength().x() - origin_x_offset - tf_point.x) / out_grid_map.getResolution();
cv_points.emplace_back(cv::Point(cv_x, cv_y));
}
cv::fillConvexPoly(filled_image, cv_points.data(), cv_points.size(), cv::Scalar(in_fill_color));
}
// convert to ROS msg
grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(filled_image,
in_grid_layer_name,
out_grid_map,
in_layer_min_value,
in_layer_max_value);
}
float SSVTreeCollider::PrimDist(udword id0, udword id1, Point& point0, Point& point1)
{
// Request vertices from the app
VertexPointers VP0;
VertexPointers VP1;
mIMesh0->GetTriangle(VP0, id0);
mIMesh1->GetTriangle(VP1, id1);
// Modified by S-cubed, Inc.
// Transform from space 0 (old : 1) to space 1 (old : 0)
// In CD, the conversion is opposite, so it is adjusted accordingly.
Point u0,u1,u2;
TransformPoint(u0, *VP0.Vertex[0], mR0to1, mT0to1);
TransformPoint(u1, *VP0.Vertex[1], mR0to1, mT0to1);
TransformPoint(u2, *VP0.Vertex[2], mR0to1, mT0to1);
// Perform triangle-triangle distance test
return TriTriDist(u0, u1, u2,
*VP1.Vertex[0], *VP1.Vertex[1], *VP1.Vertex[2],
point0, point1);
}
