Related
I am attempting to generate noise similar to perlin or value noise.
I am using stb_image_write library from here to write to image file (i write to disk as hdr and converted to png with GIMP to post on here).
This code requires C++ 20 because I am using std::lerp();
Because I am generating a linear gradient as testing I am expecting a linear gradient as output.
I know there are more steps to generating the desired noise but, this is where I'm having issues.
#include <cmath>
template <size_t size>
void noise(float seed[size][size], float output[size][size], size_t octave);
int main()
{
// generate test gradient
float seedNoise[64][64] = {};
for ( size_t x = 0; x < 64; x++ )
{
for ( size_t y = 0; y < 64; y++ )
{
seedNoise[x][y] = ((((float)x) / 64.0f + ((float)y / 64.0f)) / 2.0f);
}
}
float _map[64][64] = { 0 };
noise<64>(seedNoise, _map, 4);
}
template <size_t size>
void noise(float seed[size][size], float output[size][size], size_t octave)
{
size_t step = size / octave; // went back to this
// size_t step = (size - 1) / octave; // took this back out
for ( size_t x = 0; x <= size - step; x += step )
{
for ( size_t y = 0; y <= size - step; y += step )
{ // each octave
// extract values at corners octave from seed
float a = seed[x][y];
float b = seed[x + (step - 1)][y]; // changed this
float c = seed[x][y + (step - 1)]; // this
float d = seed[x + (step - 1)][y + (step - 1)]; // and this
for ( size_t u = 0; u < step; u++ )
{
float uStep = ((float)u) / ((float)step); // calculate x step
for ( size_t v = 0; v < step; v++ )
{ // each element in each octave
float vStep = ((float)v) / ((float)step); // calculate y step
float x1 = std::lerp(a, b, uStep); // interpolate top edge
float x2 = std::lerp(c, d, uStep); // interpolate bottom edge
float y1 = std::lerp(a, c, vStep); // interpolate left edge
float y2 = std::lerp(b, d, vStep); // interpolate right edge
float x3 = std::lerp(x1, x2, vStep); // interpolate between top and bottom edges
float y3 = std::lerp(y1, y2, uStep); // interpolate between left and right edges
float odat = (x3 + y3) / 2; // average top/bottom and left/right interpolations
output[x + u][y + v] = odat;
}
}
}
}
}
Source gradient I think this should be similar to what the output should be.
Output As you can see here the right and bottom of the output is all messed up.
new output
I think you're accessing the imago outside it's boundaries.
X and y can go up to 60 in the loops:
for ( size_t x = 0; x <= size - step; x += step )
And the you are accessing position y+step and x+step, which gives 64.
I'm trying to implement the Delaunay triangulation in C++. Currently it's working, but I'm not getting the correct amount of triangles.
I try it with 4 points in a square pattern : (0,0), (1,0), (0,1), (1,1).
Here's the algorithm I use :
std::vector<Triangle> Delaunay::triangulate(std::vector<Vec2f> &vertices) {
// Determinate the super triangle
float minX = vertices[0].getX();
float minY = vertices[0].getY();
float maxX = minX;
float maxY = minY;
for(std::size_t i = 0; i < vertices.size(); ++i) {
if (vertices[i].getX() < minX) minX = vertices[i].getX();
if (vertices[i].getY() < minY) minY = vertices[i].getY();
if (vertices[i].getX() > maxX) maxX = vertices[i].getX();
if (vertices[i].getY() > maxY) maxY = vertices[i].getY();
}
float dx = maxX - minX;
float dy = maxY - minY;
float deltaMax = std::max(dx, dy);
float midx = (minX + maxX) / 2.f;
float midy = (minY + maxY) / 2.f;
Vec2f p1(midx - 20 * deltaMax, midy - deltaMax);
Vec2f p2(midx, midy + 20 * deltaMax);
Vec2f p3(midx + 20 * deltaMax, midy - deltaMax);
// Add the super triangle vertices to the end of the vertex list
vertices.push_back(p1);
vertices.push_back(p2);
vertices.push_back(p3);
// Add the super triangle to the triangle list
std::vector<Triangle> triangleList = {Triangle(p1, p2, p3)};
// For each point in the vertex list
for(auto point = begin(vertices); point != end(vertices); point++)
{
// Initialize the edges buffer
std::vector<Edge> edgesBuff;
// For each triangles currently in the triangle list
for(auto triangle = begin(triangleList); triangle != end(triangleList);)
{
if(triangle->inCircumCircle(*point))
{
Edge tmp[3] = {triangle->getE1(), triangle->getE2(), triangle->getE3()};
edgesBuff.insert(end(edgesBuff), tmp, tmp + 3);
triangle = triangleList.erase(triangle);
}
else
{
triangle++;
}
}
// Delete all doubly specified edges from the edge buffer
// Black magic by https://github.com/MechaRage
auto ite = begin(edgesBuff), last = end(edgesBuff);
while(ite != last) {
// Search for at least one duplicate of the current element
auto twin = std::find(ite + 1, last, *ite);
if(twin != last)
// If one is found, push them all to the end.
last = std::partition(ite, last, [&ite](auto const &o){ return !(o == *ite); });
else
++ite;
}
// Remove all the duplicates, which have been shoved past "last".
edgesBuff.erase(last, end(edgesBuff));
// Add the triangle to the list
for(auto edge = begin(edgesBuff); edge != end(edgesBuff); edge++)
triangleList.push_back(Triangle(edge->getP1(), edge->getP2(), *point));
}
// Remove any triangles from the triangle list that use the supertriangle vertices
triangleList.erase(std::remove_if(begin(triangleList), end(triangleList), [p1, p2, p3](auto t){
return t.containsVertex(p1) || t.containsVertex(p2) || t.containsVertex(p3);
}), end(triangleList));
return triangleList;
}
And here's what I obtain :
Triangle:
Point x: 1 y: 0
Point x: 0 y: 0
Point x: 1 y: 1
Triangle:
Point x: 1 y: 0
Point x: 1 y: 1
Point x: 0 y: 1
Triangle:
Point x: 0 y: 0
Point x: 1 y: 1
Point x: 0 y: 1
While this would be the correct output :
Triangle:
Point x: 1 y: 0
Point x: 0 y: 0
Point x: 0 y: 1
Triangle:
Point x: 1 y: 0
Point x: 1 y: 1
Point x: 0 y: 1
I have no idea why there is a triangle with the (0, 0) and the (1, 1).
I need an outside eye to review the code and find out what's going wrong.
All the sources are on my Github repo. Feel free to fork it and to PR your code.
Thanks!
what about this implementation of Paul Bourke's Delaunay triangulation algorithm. Take a look at Triangulate() I have used this source many times without any complains
#include <iostream>
#include <stdlib.h> // for C qsort
#include <cmath>
#include <time.h> // for random
const int MaxVertices = 500;
const int MaxTriangles = 1000;
//const int n_MaxPoints = 10; // for the test programm
const double EPSILON = 0.000001;
struct ITRIANGLE{
int p1, p2, p3;
};
struct IEDGE{
int p1, p2;
};
struct XYZ{
double x, y, z;
};
int XYZCompare(const void *v1, const void *v2);
int Triangulate(int nv, XYZ pxyz[], ITRIANGLE v[], int &ntri);
int CircumCircle(double, double, double, double, double, double, double, double, double&, double&, double&);
using namespace std;
////////////////////////////////////////////////////////////////////////
// CircumCircle() :
// Return true if a point (xp,yp) is inside the circumcircle made up
// of the points (x1,y1), (x2,y2), (x3,y3)
// The circumcircle centre is returned in (xc,yc) and the radius r
// Note : A point on the edge is inside the circumcircle
////////////////////////////////////////////////////////////////////////
int CircumCircle(double xp, double yp, double x1, double y1, double x2,
double y2, double x3, double y3, double &xc, double &yc, double &r){
double m1, m2, mx1, mx2, my1, my2;
double dx, dy, rsqr, drsqr;
/* Check for coincident points */
if(abs(y1 - y2) < EPSILON && abs(y2 - y3) < EPSILON)
return(false);
if(abs(y2-y1) < EPSILON){
m2 = - (x3 - x2) / (y3 - y2);
mx2 = (x2 + x3) / 2.0;
my2 = (y2 + y3) / 2.0;
xc = (x2 + x1) / 2.0;
yc = m2 * (xc - mx2) + my2;
}else if(abs(y3 - y2) < EPSILON){
m1 = - (x2 - x1) / (y2 - y1);
mx1 = (x1 + x2) / 2.0;
my1 = (y1 + y2) / 2.0;
xc = (x3 + x2) / 2.0;
yc = m1 * (xc - mx1) + my1;
}else{
m1 = - (x2 - x1) / (y2 - y1);
m2 = - (x3 - x2) / (y3 - y2);
mx1 = (x1 + x2) / 2.0;
mx2 = (x2 + x3) / 2.0;
my1 = (y1 + y2) / 2.0;
my2 = (y2 + y3) / 2.0;
xc = (m1 * mx1 - m2 * mx2 + my2 - my1) / (m1 - m2);
yc = m1 * (xc - mx1) + my1;
}
dx = x2 - xc;
dy = y2 - yc;
rsqr = dx * dx + dy * dy;
r = sqrt(rsqr);
dx = xp - xc;
dy = yp - yc;
drsqr = dx * dx + dy * dy;
return((drsqr <= rsqr) ? true : false);
}
///////////////////////////////////////////////////////////////////////////////
// Triangulate() :
// Triangulation subroutine
// Takes as input NV vertices in array pxyz
// Returned is a list of ntri triangular faces in the array v
// These triangles are arranged in a consistent clockwise order.
// The triangle array 'v' should be malloced to 3 * nv
// The vertex array pxyz must be big enough to hold 3 more points
// The vertex array must be sorted in increasing x values say
//
// qsort(p,nv,sizeof(XYZ),XYZCompare);
///////////////////////////////////////////////////////////////////////////////
int Triangulate(int nv, XYZ pxyz[], ITRIANGLE v[], int &ntri){
int *complete = NULL;
IEDGE *edges = NULL;
IEDGE *p_EdgeTemp;
int nedge = 0;
int trimax, emax = 200;
int status = 0;
int inside;
int i, j, k;
double xp, yp, x1, y1, x2, y2, x3, y3, xc, yc, r;
double xmin, xmax, ymin, ymax, xmid, ymid;
double dx, dy, dmax;
/* Allocate memory for the completeness list, flag for each triangle */
trimax = 4 * nv;
complete = new int[trimax];
/* Allocate memory for the edge list */
edges = new IEDGE[emax];
/*
Find the maximum and minimum vertex bounds.
This is to allow calculation of the bounding triangle
*/
xmin = pxyz[0].x;
ymin = pxyz[0].y;
xmax = xmin;
ymax = ymin;
for(i = 1; i < nv; i++){
if (pxyz[i].x < xmin) xmin = pxyz[i].x;
if (pxyz[i].x > xmax) xmax = pxyz[i].x;
if (pxyz[i].y < ymin) ymin = pxyz[i].y;
if (pxyz[i].y > ymax) ymax = pxyz[i].y;
}
dx = xmax - xmin;
dy = ymax - ymin;
dmax = (dx > dy) ? dx : dy;
xmid = (xmax + xmin) / 2.0;
ymid = (ymax + ymin) / 2.0;
/*
Set up the supertriangle
his is a triangle which encompasses all the sample points.
The supertriangle coordinates are added to the end of the
vertex list. The supertriangle is the first triangle in
the triangle list.
*/
pxyz[nv+0].x = xmid - 20 * dmax;
pxyz[nv+0].y = ymid - dmax;
pxyz[nv+1].x = xmid;
pxyz[nv+1].y = ymid + 20 * dmax;
pxyz[nv+2].x = xmid + 20 * dmax;
pxyz[nv+2].y = ymid - dmax;
v[0].p1 = nv;
v[0].p2 = nv+1;
v[0].p3 = nv+2;
complete[0] = false;
ntri = 1;
/*
Include each point one at a time into the existing mesh
*/
for(i = 0; i < nv; i++){
xp = pxyz[i].x;
yp = pxyz[i].y;
nedge = 0;
/*
Set up the edge buffer.
If the point (xp,yp) lies inside the circumcircle then the
three edges of that triangle are added to the edge buffer
and that triangle is removed.
*/
for(j = 0; j < ntri; j++){
if(complete[j])
continue;
x1 = pxyz[v[j].p1].x;
y1 = pxyz[v[j].p1].y;
x2 = pxyz[v[j].p2].x;
y2 = pxyz[v[j].p2].y;
x3 = pxyz[v[j].p3].x;
y3 = pxyz[v[j].p3].y;
inside = CircumCircle(xp, yp, x1, y1, x2, y2, x3, y3, xc, yc, r);
if (xc + r < xp)
// Suggested
// if (xc + r + EPSILON < xp)
complete[j] = true;
if(inside){
/* Check that we haven't exceeded the edge list size */
if(nedge + 3 >= emax){
emax += 100;
p_EdgeTemp = new IEDGE[emax];
for (int i = 0; i < nedge; i++) { // Fix by John Bowman
p_EdgeTemp[i] = edges[i];
}
delete []edges;
edges = p_EdgeTemp;
}
edges[nedge+0].p1 = v[j].p1;
edges[nedge+0].p2 = v[j].p2;
edges[nedge+1].p1 = v[j].p2;
edges[nedge+1].p2 = v[j].p3;
edges[nedge+2].p1 = v[j].p3;
edges[nedge+2].p2 = v[j].p1;
nedge += 3;
v[j] = v[ntri-1];
complete[j] = complete[ntri-1];
ntri--;
j--;
}
}
/*
Tag multiple edges
Note: if all triangles are specified anticlockwise then all
interior edges are opposite pointing in direction.
*/
for(j = 0; j < nedge - 1; j++){
for(k = j + 1; k < nedge; k++){
if((edges[j].p1 == edges[k].p2) && (edges[j].p2 == edges[k].p1)){
edges[j].p1 = -1;
edges[j].p2 = -1;
edges[k].p1 = -1;
edges[k].p2 = -1;
}
/* Shouldn't need the following, see note above */
if((edges[j].p1 == edges[k].p1) && (edges[j].p2 == edges[k].p2)){
edges[j].p1 = -1;
edges[j].p2 = -1;
edges[k].p1 = -1;
edges[k].p2 = -1;
}
}
}
/*
Form new triangles for the current point
Skipping over any tagged edges.
All edges are arranged in clockwise order.
*/
for(j = 0; j < nedge; j++) {
if(edges[j].p1 < 0 || edges[j].p2 < 0)
continue;
v[ntri].p1 = edges[j].p1;
v[ntri].p2 = edges[j].p2;
v[ntri].p3 = i;
complete[ntri] = false;
ntri++;
}
}
/*
Remove triangles with supertriangle vertices
These are triangles which have a vertex number greater than nv
*/
for(i = 0; i < ntri; i++) {
if(v[i].p1 >= nv || v[i].p2 >= nv || v[i].p3 >= nv) {
v[i] = v[ntri-1];
ntri--;
i--;
}
}
delete[] edges;
delete[] complete;
return 0;
}
int XYZCompare(const void *v1, const void *v2){
XYZ *p1, *p2;
p1 = (XYZ*)v1;
p2 = (XYZ*)v2;
if(p1->x < p2->x)
return(-1);
else if(p1->x > p2->x)
return(1);
else
return(0);
}
I didn't go with a debugger, but from the resulting triangles it seems that this is an accuracy/ambiguity problem.
When you are triangulating a square there are two ways to split it into triangles and both are OK from Delaunay criteria (circumscribed circle center is on border of triangle).
So if you evaluate every triangle independently you may sometimes get even 4 triangles (depending on implementation).
Normally in such cases I recommend to build algorithm as a series of questions which cannot produce contradicting answers. In this case the question is "to which point goes triangle based on edge (1,0)-(1,1)". But often this requires significant changes to the algorithm.
A quick fix usually involves adding some tolerances for comparisons and extra checks (like non-intersecting triangles). But usually it just makes problems rarer.
Most likely you didn't delete all the double edges, especially not the edges from same triangles but with vertices only in another order. The correct function is in the answer from #cMinor.
I have two 3D point clouds, and I'd like to use opencv to find the rigid transformation matrix (translation, rotation, constant scaling among all 3 axes).
I've found an estimateRigidTransformation function, but it's only for 2D points apparently
In addition, I've found estimateAffine3D, but it doesn't seem to support rigid transformation mode.
Do I need to just write my own rigid transformation function?
I did not find the required functionality in OpenCV so I have written my own implementation. Based on ideas from OpenSFM.
cv::Vec3d
CalculateMean(const cv::Mat_<cv::Vec3d> &points)
{
cv::Mat_<cv::Vec3d> result;
cv::reduce(points, result, 0, CV_REDUCE_AVG);
return result(0, 0);
}
cv::Mat_<double>
FindRigidTransform(const cv::Mat_<cv::Vec3d> &points1, const cv::Mat_<cv::Vec3d> points2)
{
/* Calculate centroids. */
cv::Vec3d t1 = -CalculateMean(points1);
cv::Vec3d t2 = -CalculateMean(points2);
cv::Mat_<double> T1 = cv::Mat_<double>::eye(4, 4);
T1(0, 3) = t1[0];
T1(1, 3) = t1[1];
T1(2, 3) = t1[2];
cv::Mat_<double> T2 = cv::Mat_<double>::eye(4, 4);
T2(0, 3) = -t2[0];
T2(1, 3) = -t2[1];
T2(2, 3) = -t2[2];
/* Calculate covariance matrix for input points. Also calculate RMS deviation from centroid
* which is used for scale calculation.
*/
cv::Mat_<double> C(3, 3, 0.0);
double p1Rms = 0, p2Rms = 0;
for (int ptIdx = 0; ptIdx < points1.rows; ptIdx++) {
cv::Vec3d p1 = points1(ptIdx, 0) + t1;
cv::Vec3d p2 = points2(ptIdx, 0) + t2;
p1Rms += p1.dot(p1);
p2Rms += p2.dot(p2);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
C(i, j) += p2[i] * p1[j];
}
}
}
cv::Mat_<double> u, s, vh;
cv::SVD::compute(C, s, u, vh);
cv::Mat_<double> R = u * vh;
if (cv::determinant(R) < 0) {
R -= u.col(2) * (vh.row(2) * 2.0);
}
double scale = sqrt(p2Rms / p1Rms);
R *= scale;
cv::Mat_<double> M = cv::Mat_<double>::eye(4, 4);
R.copyTo(M.colRange(0, 3).rowRange(0, 3));
cv::Mat_<double> result = T2 * M * T1;
result /= result(3, 3);
return result.rowRange(0, 3);
}
I've found PCL to be a nice adjunct to OpenCV. Take a look at their Iterative Closest Point (ICP) example. The provided example registers the two point clouds and then displays the rigid transformation.
Here's my rmsd code:
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>
typedef struct
{
float m[4][4];
} MATRIX;
#define vdiff2(a,b) ( ((a)[0]-(b)[0]) * ((a)[0]-(b)[0]) + \
((a)[1]-(b)[1]) * ((a)[1]-(b)[1]) + \
((a)[2]-(b)[2]) * ((a)[2]-(b)[2]) )
static double alignedrmsd(float *v1, float *v2, int N);
static void centroid(float *ret, float *v, int N);
static int getalignmtx(float *v1, float *v2, int N, MATRIX *mtx);
static void crossproduct(float *ans, float *pt1, float *pt2);
static void mtx_root(MATRIX *mtx);
static int almostequal(MATRIX *a, MATRIX *b);
static void mulpt(MATRIX *mtx, float *pt);
static void mtx_mul(MATRIX *ans, MATRIX *x, MATRIX *y);
static void mtx_identity(MATRIX *mtx);
static void mtx_trans(MATRIX *mtx, float x, float y, float z);
static int mtx_invert(float *mtx, int N);
static float absmaxv(float *v, int N);
/*
calculate rmsd between two structures
Params: v1 - first set of points
v2 - second set of points
N - number of points
mtx - return for transfrom matrix used to align structures
Returns: rmsd score
Notes: mtx can be null. Transform will be rigid. Inputs must
be previously aligned for sequence alignment
*/
double rmsd(float *v1, float *v2, int N, float *mtx)
{
float cent1[3];
float cent2[3];
MATRIX tmtx;
MATRIX tempmtx;
MATRIX move1;
MATRIX move2;
int i;
double answer;
float *temp1 = 0;
float *temp2 = 0;
int err;
assert(N > 3);
temp1 = malloc(N * 3 * sizeof(float));
temp2 = malloc(N * 3 * sizeof(float));
if(!temp1 || !temp2)
goto error_exit;
centroid(cent1, v1, N);
centroid(cent2, v2, N);
for(i=0;i<N;i++)
{
temp1[i*3+0] = v1[i*3+0] - cent1[0];
temp1[i*3+1] = v1[i*3+1] - cent1[1];
temp1[i*3+2] = v1[i*3+2] - cent1[2];
temp2[i*3+0] = v2[i*3+0] - cent2[0];
temp2[i*3+1] = v2[i*3+1] - cent2[1];
temp2[i*3+2] = v2[i*3+2] - cent2[2];
}
err = getalignmtx(temp1, temp2, N, &tmtx);
if(err == -1)
goto error_exit;
mtx_trans(&move1, -cent2[0], -cent2[1], -cent2[2]);
mtx_mul(&tempmtx, &move1, &tmtx);
mtx_trans(&move2, cent1[0], cent1[1], cent1[2]);
mtx_mul(&tmtx, &tempmtx, &move2);
memcpy(temp2, v2, N * sizeof(float) * 3);
for(i=0;i<N;i++)
mulpt(&tmtx, temp2 + i * 3);
answer = alignedrmsd(v1, temp2, N);
free(temp1);
free(temp2);
if(mtx)
memcpy(mtx, &tmtx.m, 16 * sizeof(float));
return answer;
error_exit:
free(temp1);
free(temp2);
if(mtx)
{
for(i=0;i<16;i++)
mtx[i] = 0;
}
return sqrt(-1.0);
}
/*
calculate rmsd between two aligned structures (trivial)
Params: v1 - first structure
v2 - second structure
N - number of points
Returns: rmsd
*/
static double alignedrmsd(float *v1, float *v2, int N)
{
double answer =0;
int i;
for(i=0;i<N;i++)
answer += vdiff2(v1 + i *3, v2 + i * 3);
return sqrt(answer/N);
}
/*
compute the centroid
*/
static void centroid(float *ret, float *v, int N)
{
int i;
ret[0] = 0;
ret[1] = 0;
ret[2] = 0;
for(i=0;i<N;i++)
{
ret[0] += v[i*3+0];
ret[1] += v[i*3+1];
ret[2] += v[i*3+2];
}
ret[0] /= N;
ret[1] /= N;
ret[2] /= N;
}
/*
get the matrix needed to align two structures
Params: v1 - reference structure
v2 - structure to align
N - number of points
mtx - return for rigid body alignment matrix
Notes: only calculates rotation part of matrix.
assumes input has been aligned to centroids
*/
static int getalignmtx(float *v1, float *v2, int N, MATRIX *mtx)
{
MATRIX A = { {{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,1}} };
MATRIX At;
MATRIX Ainv;
MATRIX temp;
float tv[3];
float tw[3];
float tv2[3];
float tw2[3];
int k, i, j;
int flag = 0;
float correction;
correction = absmaxv(v1, N * 3) * absmaxv(v2, N * 3);
for(k=0;k<N;k++)
for(i=0;i<3;i++)
for(j=0;j<3;j++)
A.m[i][j] += (v1[k*3+i] * v2[k*3+j])/correction;
while(flag < 3)
{
for(i=0;i<4;i++)
for(j=0;j<4;j++)
At.m[i][j] = A.m[j][i];
memcpy(&Ainv, &A, sizeof(MATRIX));
/* this will happen if all points are in a plane */
if( mtx_invert((float *) &Ainv, 4) == -1)
{
if(flag == 0)
{
crossproduct(tv, v1, v1+3);
crossproduct(tw, v2, v2+3);
}
else
{
crossproduct(tv2, tv, v1);
crossproduct(tw2, tw, v2);
memcpy(tv, tv2, 3 * sizeof(float));
memcpy(tw, tw2, 3 * sizeof(float));
}
for(i=0;i<3;i++)
for(j=0;j<3;j++)
A.m[i][j] += tv[i] * tw[j];
flag++;
}
else
flag = 5;
}
if(flag != 5)
return -1;
mtx_mul(&temp, &At, &A);
mtx_root(&temp);
mtx_mul(mtx, &temp, &Ainv);
return 0;
}
/*
get the crossproduct of two vectors.
Params: ans - return pinter for answer.
pt1 - first vector
pt2 - second vector.
Notes: crossproduct is at right angles to the two vectors.
*/
static void crossproduct(float *ans, float *pt1, float *pt2)
{
ans[0] = pt1[1] * pt2[2] - pt1[2] * pt2[1];
ans[1] = pt1[0] * pt2[2] - pt1[2] * pt2[0];
ans[2] = pt1[0] * pt2[1] - pt1[1] * pt2[0];
}
/*
Denman-Beavers square root iteration
*/
static void mtx_root(MATRIX *mtx)
{
MATRIX Y = *mtx;
MATRIX Z;
MATRIX Y1;
MATRIX Z1;
MATRIX invY;
MATRIX invZ;
MATRIX Y2;
int iter = 0;
int i, ii;
mtx_identity(&Z);
do
{
invY = Y;
invZ = Z;
if( mtx_invert((float *) &invY, 4) == -1)
return;
if( mtx_invert((float *) &invZ, 4) == -1)
return;
for(i=0;i<4;i++)
for(ii=0;ii<4;ii++)
{
Y1.m[i][ii] = 0.5 * (Y.m[i][ii] + invZ.m[i][ii]);
Z1.m[i][ii] = 0.5 * (Z.m[i][ii] + invY.m[i][ii]);
}
Y = Y1;
Z = Z1;
mtx_mul(&Y2, &Y, &Y);
}
while(!almostequal(&Y2, mtx) && iter++ < 20 );
*mtx = Y;
}
/*
Check two matrices for near-enough equality
Params: a - first matrix
b - second matrix
Returns: 1 if almost equal, else 0, epsilon 0.0001f.
*/
static int almostequal(MATRIX *a, MATRIX *b)
{
int i, ii;
float epsilon = 0.001f;
for(i=0;i<4;i++)
for(ii=0;ii<4;ii++)
if(fabs(a->m[i][ii] - b->m[i][ii]) > epsilon)
return 0;
return 1;
}
/*
multiply a point by a matrix.
Params: mtx - matrix
pt - the point (transformed)
*/
static void mulpt(MATRIX *mtx, float *pt)
{
float ans[4] = {0};
int i;
int ii;
for(i=0;i<4;i++)
{
for(ii=0;ii<3;ii++)
{
ans[i] += pt[ii] * mtx->m[ii][i];
}
ans[i] += mtx->m[3][i];
}
pt[0] = ans[0];
pt[1] = ans[1];
pt[2] = ans[2];
}
/*
multiply two matrices.
Params: ans - return pointer for answer.
x - first matrix
y - second matrix.
Notes: ans may not be equal to x or y.
*/
static void mtx_mul(MATRIX *ans, MATRIX *x, MATRIX *y)
{
int i;
int ii;
int iii;
for(i=0;i<4;i++)
for(ii=0;ii<4;ii++)
{
ans->m[i][ii] = 0;
for(iii=0;iii<4;iii++)
ans->m[i][ii] += x->m[i][iii] * y->m[iii][ii];
}
}
/*
create an identity matrix.
Params: mtx - return pointer.
*/
static void mtx_identity(MATRIX *mtx)
{
int i;
int ii;
for(i=0;i<4;i++)
for(ii=0;ii<4;ii++)
{
if(i==ii)
mtx->m[i][ii] = 1.0f;
else
mtx->m[i][ii] = 0;
}
}
/*
create a translation matrix.
Params: mtx - return pointer for matrix.
x - x translation.
y - y translation.
z - z translation
*/
static void mtx_trans(MATRIX *mtx, float x, float y, float z)
{
mtx->m[0][0] = 1;
mtx->m[0][1] = 0;
mtx->m[0][2] = 0;
mtx->m[0][3] = 0;
mtx->m[1][0] = 0;
mtx->m[1][1] = 1;
mtx->m[1][2] = 0;
mtx->m[1][3] = 0;
mtx->m[2][0] = 0;
mtx->m[2][1] = 0;
mtx->m[2][2] = 1;
mtx->m[2][3] = 0;
mtx->m[3][0] = x;
mtx->m[3][1] = y;
mtx->m[3][2] = z;
mtx->m[3][3] = 1;
}
/*
matrix invert routine
Params: mtx - the matrix in raw format, in/out
N - width and height
Returns: 0 on success, -1 on fail
*/
static int mtx_invert(float *mtx, int N)
{
int indxc[100]; /* these 100s are the only restriction on matrix size */
int indxr[100];
int ipiv[100];
int i, j, k;
int irow, icol;
double big;
double pinv;
int l, ll;
double dum;
double temp;
assert(N <= 100);
for(i=0;i<N;i++)
ipiv[i] = 0;
for(i=0;i<N;i++)
{
big = 0.0;
/* find biggest element */
for(j=0;j<N;j++)
if(ipiv[j] != 1)
for(k=0;k<N;k++)
if(ipiv[k] == 0)
if(fabs(mtx[j*N+k]) >= big)
{
big = fabs(mtx[j*N+k]);
irow = j;
icol = k;
}
ipiv[icol]=1;
if(irow != icol)
for(l=0;l<N;l++)
{
temp = mtx[irow * N + l];
mtx[irow * N + l] = mtx[icol * N + l];
mtx[icol * N + l] = temp;
}
indxr[i] = irow;
indxc[i] = icol;
/* if biggest element is zero matrix is singular, bail */
if(mtx[icol* N + icol] == 0)
goto error_exit;
pinv = 1.0/mtx[icol * N + icol];
mtx[icol * N + icol] = 1.0;
for(l=0;l<N;l++)
mtx[icol * N + l] *= pinv;
for(ll=0;ll<N;ll++)
if(ll != icol)
{
dum = mtx[ll * N + icol];
mtx[ll * N + icol] = 0.0;
for(l=0;l<N;l++)
mtx[ll * N + l] -= mtx[icol * N + l]*dum;
}
}
/* unscramble matrix */
for (l=N-1;l>=0;l--)
{
if (indxr[l] != indxc[l])
for (k=0;k<N;k++)
{
temp = mtx[k * N + indxr[l]];
mtx[k * N + indxr[l]] = mtx[k * N + indxc[l]];
mtx[k * N + indxc[l]] = temp;
}
}
return 0;
error_exit:
return -1;
}
/*
get the asolute maximum of an array
*/
static float absmaxv(float *v, int N)
{
float answer;
int i;
for(i=0;i<N;i++)
if(answer < fabs(v[i]))
answer = fabs(v[i]);
return answer;
}
#include <stdio.h>
/*
debug utlitiy
*/
static void printmtx(FILE *fp, MATRIX *mtx)
{
int i, ii;
for(i=0;i<4;i++)
{
for(ii=0;ii<4;ii++)
fprintf(fp, "%f, ", mtx->m[i][ii]);
fprintf(fp, "\n");
}
}
int rmsdmain(void)
{
float one[4*3] = {0,0,0, 1,0,0, 2,1,0, 0,3,1};
float two[4*3] = {0,0,0, 0,1,0, 1,2,0, 3,0,1};
MATRIX mtx;
double diff;
int i;
diff = rmsd(one, two, 4, (float *) &mtx.m);
printf("%f\n", diff);
printmtx(stdout, &mtx);
for(i=0;i<4;i++)
{
mulpt(&mtx, two + i * 3);
printf("%f %f %f\n", two[i*3], two[i*3+1], two[i*3+2]);
}
return 0;
}
I took #vagran's implementation and added RANSAC on top of it, since estimateRigidTransform2d does it and it was helpful for me since my data is noisy. (Note: This code doesn't have constant scaling along all 3 axes; you can add it back in easily by comparing to vargran's).
cv::Vec3f CalculateMean(const cv::Mat_<cv::Vec3f> &points)
{
if(points.size().height == 0){
return 0;
}
assert(points.size().width == 1);
double mx = 0.0;
double my = 0.0;
double mz = 0.0;
int n_points = points.size().height;
for(int i = 0; i < n_points; i++){
double x = double(points(i)[0]);
double y = double(points(i)[1]);
double z = double(points(i)[2]);
mx += x;
my += y;
mz += z;
}
return cv::Vec3f(mx/n_points, my/n_points, mz/n_points);
}
cv::Mat_<double>
FindRigidTransform(const cv::Mat_<cv::Vec3f> &points1, const cv::Mat_<cv::Vec3f> points2)
{
/* Calculate centroids. */
cv::Vec3f t1 = CalculateMean(points1);
cv::Vec3f t2 = CalculateMean(points2);
cv::Mat_<double> T1 = cv::Mat_<double>::eye(4, 4);
T1(0, 3) = double(-t1[0]);
T1(1, 3) = double(-t1[1]);
T1(2, 3) = double(-t1[2]);
cv::Mat_<double> T2 = cv::Mat_<double>::eye(4, 4);
T2(0, 3) = double(t2[0]);
T2(1, 3) = double(t2[1]);
T2(2, 3) = double(t2[2]);
/* Calculate covariance matrix for input points. Also calculate RMS deviation from centroid
* which is used for scale calculation.
*/
cv::Mat_<double> C(3, 3, 0.0);
for (int ptIdx = 0; ptIdx < points1.rows; ptIdx++) {
cv::Vec3f p1 = points1(ptIdx) - t1;
cv::Vec3f p2 = points2(ptIdx) - t2;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
C(i, j) += double(p2[i] * p1[j]);
}
}
}
cv::Mat_<double> u, s, vt;
cv::SVD::compute(C, s, u, vt);
cv::Mat_<double> R = u * vt;
if (cv::determinant(R) < 0) {
R -= u.col(2) * (vt.row(2) * 2.0);
}
cv::Mat_<double> M = cv::Mat_<double>::eye(4, 4);
R.copyTo(M.colRange(0, 3).rowRange(0, 3));
cv::Mat_<double> result = T2 * M * T1;
result /= result(3, 3);
return result;
}
cv::Mat_<double> RANSACFindRigidTransform(const cv::Mat_<cv::Vec3f> &points1, const cv::Mat_<cv::Vec3f> &points2)
{
cv::Mat points1Homo;
cv::convertPointsToHomogeneous(points1, points1Homo);
int iterations = 100;
int min_n_points = 3;
int n_points = points1.size().height;
std::vector<int> range(n_points);
cv::Mat_<double> best;
int best_inliers = -1;
// inlier points should be projected within this many units
float threshold = .02;
std::iota(range.begin(), range.end(), 0);
auto gen = std::mt19937{std::random_device{}()};
for(int i = 0; i < iterations; i++) {
std::shuffle(range.begin(), range.end(), gen);
cv::Mat_<cv::Vec3f> points1subset(min_n_points, 1, cv::Vec3f(0,0,0));
cv::Mat_<cv::Vec3f> points2subset(min_n_points, 1, cv::Vec3f(0,0,0));
for(int j = 0; j < min_n_points; j++) {
points1subset(j) = points1(range[j]);
points2subset(j) = points2(range[j]);
}
cv::Mat_<float> rigidT = FindRigidTransform(points1subset, points2subset);
cv::Mat_<float> rigidT_float = cv::Mat::eye(4, 4, CV_32F);
rigidT.convertTo(rigidT_float, CV_32F);
std::vector<int> inliers;
for(int j = 0; j < n_points; j++) {
cv::Mat_<float> t1_3d = rigidT_float * cv::Mat_<float>(points1Homo.at<cv::Vec4f>(j));
if(t1_3d(3) == 0) {
continue; // Avoid 0 division
}
float dx = (t1_3d(0)/t1_3d(3) - points2(j)[0]);
float dy = (t1_3d(1)/t1_3d(3) - points2(j)[1]);
float dz = (t1_3d(2)/t1_3d(3) - points2(j)[2]);
float square_dist = dx * dx + dy * dy + dz * dz;
if(square_dist < threshold * threshold){
inliers.push_back(j);
}
}
int n_inliers = inliers.size();
if(n_inliers > best_inliers) {
best_inliers = n_inliers;
best = rigidT;
}
}
return best;
}
#vagran Thanks for the code! Seems to work very well.
I do have a little terminology suggestion though. Since you are estimating and applying a scale during the transformation, it is a 7-parameter transformation, or Helmert / similarity transformation. And in a rigid transformation, no scaling is applied because all Euclidiean distances need to be reserved.
I would've added this as comment, but don't have enough points.. D: sorry for that.
rigid transformation: https://en.wikipedia.org/wiki/Rigid_transformation
Helmert transformation: https://www.researchgate.net/publication/322841143_Parameter_estimation_in_3D_affine_and_similarity_transformation_implementation_of_variance_component_estimation
To get the center, I have tried, for each vertex, to add to the total, divide by the number of vertices.
I've also tried to find the topmost, bottommost -> get midpoint... find leftmost, rightmost, find the midpoint.
Both of these did not return the perfect center because I'm relying on the center to scale a polygon.
I want to scale my polygons, so I may put a border around them.
What is the best way to find the centroid of a polygon given that the polygon may be concave, convex and have many many sides of various lengths?
The formula is given here for vertices sorted by their occurance along the polygon's perimeter.
For those having difficulty understanding the sigma notation in those formulas, here is some C++ code showing how to do the computation:
#include <iostream>
struct Point2D
{
double x;
double y;
};
Point2D compute2DPolygonCentroid(const Point2D* vertices, int vertexCount)
{
Point2D centroid = {0, 0};
double signedArea = 0.0;
double x0 = 0.0; // Current vertex X
double y0 = 0.0; // Current vertex Y
double x1 = 0.0; // Next vertex X
double y1 = 0.0; // Next vertex Y
double a = 0.0; // Partial signed area
// For all vertices except last
int i=0;
for (i=0; i<vertexCount-1; ++i)
{
x0 = vertices[i].x;
y0 = vertices[i].y;
x1 = vertices[i+1].x;
y1 = vertices[i+1].y;
a = x0*y1 - x1*y0;
signedArea += a;
centroid.x += (x0 + x1)*a;
centroid.y += (y0 + y1)*a;
}
// Do last vertex separately to avoid performing an expensive
// modulus operation in each iteration.
x0 = vertices[i].x;
y0 = vertices[i].y;
x1 = vertices[0].x;
y1 = vertices[0].y;
a = x0*y1 - x1*y0;
signedArea += a;
centroid.x += (x0 + x1)*a;
centroid.y += (y0 + y1)*a;
signedArea *= 0.5;
centroid.x /= (6.0*signedArea);
centroid.y /= (6.0*signedArea);
return centroid;
}
int main()
{
Point2D polygon[] = {{0.0,0.0}, {0.0,10.0}, {10.0,10.0}, {10.0,0.0}};
size_t vertexCount = sizeof(polygon) / sizeof(polygon[0]);
Point2D centroid = compute2DPolygonCentroid(polygon, vertexCount);
std::cout << "Centroid is (" << centroid.x << ", " << centroid.y << ")\n";
}
I've only tested this for a square polygon in the upper-right x/y quadrant.
If you don't mind performing two (potentially expensive) extra modulus operations in each iteration, then you can simplify the previous compute2DPolygonCentroid function to the following:
Point2D compute2DPolygonCentroid(const Point2D* vertices, int vertexCount)
{
Point2D centroid = {0, 0};
double signedArea = 0.0;
double x0 = 0.0; // Current vertex X
double y0 = 0.0; // Current vertex Y
double x1 = 0.0; // Next vertex X
double y1 = 0.0; // Next vertex Y
double a = 0.0; // Partial signed area
// For all vertices
int i=0;
for (i=0; i<vertexCount; ++i)
{
x0 = vertices[i].x;
y0 = vertices[i].y;
x1 = vertices[(i+1) % vertexCount].x;
y1 = vertices[(i+1) % vertexCount].y;
a = x0*y1 - x1*y0;
signedArea += a;
centroid.x += (x0 + x1)*a;
centroid.y += (y0 + y1)*a;
}
signedArea *= 0.5;
centroid.x /= (6.0*signedArea);
centroid.y /= (6.0*signedArea);
return centroid;
}
The centroid can be calculated as the weighted sum of the centroids of the triangles it can be partitioned to.
Here is the C source code for such an algorithm:
/*
Written by Joseph O'Rourke
orourke#cs.smith.edu
October 27, 1995
Computes the centroid (center of gravity) of an arbitrary
simple polygon via a weighted sum of signed triangle areas,
weighted by the centroid of each triangle.
Reads x,y coordinates from stdin.
NB: Assumes points are entered in ccw order!
E.g., input for square:
0 0
10 0
10 10
0 10
This solves Exercise 12, p.47, of my text,
Computational Geometry in C. See the book for an explanation
of why this works. Follow links from
http://cs.smith.edu/~orourke/
*/
#include <stdio.h>
#define DIM 2 /* Dimension of points */
typedef int tPointi[DIM]; /* type integer point */
typedef double tPointd[DIM]; /* type double point */
#define PMAX 1000 /* Max # of pts in polygon */
typedef tPointi tPolygoni[PMAX];/* type integer polygon */
int Area2( tPointi a, tPointi b, tPointi c );
void FindCG( int n, tPolygoni P, tPointd CG );
int ReadPoints( tPolygoni P );
void Centroid3( tPointi p1, tPointi p2, tPointi p3, tPointi c );
void PrintPoint( tPointd p );
int main()
{
int n;
tPolygoni P;
tPointd CG;
n = ReadPoints( P );
FindCG( n, P ,CG);
printf("The cg is ");
PrintPoint( CG );
}
/*
Returns twice the signed area of the triangle determined by a,b,c,
positive if a,b,c are oriented ccw, and negative if cw.
*/
int Area2( tPointi a, tPointi b, tPointi c )
{
return
(b[0] - a[0]) * (c[1] - a[1]) -
(c[0] - a[0]) * (b[1] - a[1]);
}
/*
Returns the cg in CG. Computes the weighted sum of
each triangle's area times its centroid. Twice area
and three times centroid is used to avoid division
until the last moment.
*/
void FindCG( int n, tPolygoni P, tPointd CG )
{
int i;
double A2, Areasum2 = 0; /* Partial area sum */
tPointi Cent3;
CG[0] = 0;
CG[1] = 0;
for (i = 1; i < n-1; i++) {
Centroid3( P[0], P[i], P[i+1], Cent3 );
A2 = Area2( P[0], P[i], P[i+1]);
CG[0] += A2 * Cent3[0];
CG[1] += A2 * Cent3[1];
Areasum2 += A2;
}
CG[0] /= 3 * Areasum2;
CG[1] /= 3 * Areasum2;
return;
}
/*
Returns three times the centroid. The factor of 3 is
left in to permit division to be avoided until later.
*/
void Centroid3( tPointi p1, tPointi p2, tPointi p3, tPointi c )
{
c[0] = p1[0] + p2[0] + p3[0];
c[1] = p1[1] + p2[1] + p3[1];
return;
}
void PrintPoint( tPointd p )
{
int i;
putchar('(');
for ( i=0; i<DIM; i++) {
printf("%f",p[i]);
if (i != DIM - 1) putchar(',');
}
putchar(')');
putchar('\n');
}
/*
Reads in the coordinates of the vertices of a polygon from stdin,
puts them into P, and returns n, the number of vertices.
The input is assumed to be pairs of whitespace-separated coordinates,
one pair per line. The number of points is not part of the input.
*/
int ReadPoints( tPolygoni P )
{
int n = 0;
printf("Polygon:\n");
printf(" i x y\n");
while ( (n < PMAX) && (scanf("%d %d",&P[n][0],&P[n][1]) != EOF) ) {
printf("%3d%4d%4d\n", n, P[n][0], P[n][1]);
++n;
}
if (n < PMAX)
printf("n = %3d vertices read\n",n);
else
printf("Error in ReadPoints:\too many points; max is %d\n", PMAX);
putchar('\n');
return n;
}
There's a polygon centroid article on the CGAFaq (comp.graphics.algorithms FAQ) wiki that explains it.
boost::geometry::centroid(your_polygon, p);
Here is Emile Cormier's algorithm without duplicated code or expensive modulus operations, best of both worlds:
#include <iostream>
using namespace std;
struct Point2D
{
double x;
double y;
};
Point2D compute2DPolygonCentroid(const Point2D* vertices, int vertexCount)
{
Point2D centroid = {0, 0};
double signedArea = 0.0;
double x0 = 0.0; // Current vertex X
double y0 = 0.0; // Current vertex Y
double x1 = 0.0; // Next vertex X
double y1 = 0.0; // Next vertex Y
double a = 0.0; // Partial signed area
int lastdex = vertexCount-1;
const Point2D* prev = &(vertices[lastdex]);
const Point2D* next;
// For all vertices in a loop
for (int i=0; i<vertexCount; ++i)
{
next = &(vertices[i]);
x0 = prev->x;
y0 = prev->y;
x1 = next->x;
y1 = next->y;
a = x0*y1 - x1*y0;
signedArea += a;
centroid.x += (x0 + x1)*a;
centroid.y += (y0 + y1)*a;
prev = next;
}
signedArea *= 0.5;
centroid.x /= (6.0*signedArea);
centroid.y /= (6.0*signedArea);
return centroid;
}
int main()
{
Point2D polygon[] = {{0.0,0.0}, {0.0,10.0}, {10.0,10.0}, {10.0,0.0}};
size_t vertexCount = sizeof(polygon) / sizeof(polygon[0]);
Point2D centroid = compute2DPolygonCentroid(polygon, vertexCount);
std::cout << "Centroid is (" << centroid.x << ", " << centroid.y << ")\n";
}
Break it into triangles, find the area and centroid of each, then calculate the average of all the partial centroids using the partial areas as weights. With concavity some of the areas could be negative.
I'd like to implement a Bézier curve. I've done this in C# before, but I'm totally unfamiliar with the C++ libraries. How should I go about creating a quadratic curve?
void printQuadCurve(float delta, Vector2f p0, Vector2f p1, Vector2f p2);
Clearly we'd need to use linear interpolation, but does this exist in the standard math library? If not, where can I find it?
I'm using Linux.
Recently I ran across the same question and wanted to implemented it on my own.
This image from Wikipedia helped me:
The following code is written in C++ and shows how to compute a quadratic bezier.
int getPt( int n1 , int n2 , float perc )
{
int diff = n2 - n1;
return n1 + ( diff * perc );
}
for( float i = 0 ; i < 1 ; i += 0.01 )
{
// The Green Line
xa = getPt( x1 , x2 , i );
ya = getPt( y1 , y2 , i );
xb = getPt( x2 , x3 , i );
yb = getPt( y2 , y3 , i );
// The Black Dot
x = getPt( xa , xb , i );
y = getPt( ya , yb , i );
drawPixel( x , y , COLOR_RED );
}
With (x1|y1), (x2|y2) and (x3|y3) being P0, P1 and P2 in the image. Just for showing the basic idea...
For the ones who ask for the cubic bezier, it just works analogue (also from Wikipedia):
This answer provides Code for it.
Here is a general implementation for a curve with any number of points.
vec2 getBezierPoint( vec2* points, int numPoints, float t ) {
vec2* tmp = new vec2[numPoints];
memcpy(tmp, points, numPoints * sizeof(vec2));
int i = numPoints - 1;
while (i > 0) {
for (int k = 0; k < i; k++)
tmp[k] = tmp[k] + t * ( tmp[k+1] - tmp[k] );
i--;
}
vec2 answer = tmp[0];
delete[] tmp;
return answer;
}
Note that it uses heap memory for a temporary array which is not all that efficient. If you only need to deal with a fixed number of points you could hard-code the numPoints value and use stack memory instead.
Of course, the above assumes you have a vec2 structure and operators for it like this:
struct vec2 {
float x, y;
vec2(float x, float y) : x(x), y(y) {}
};
vec2 operator + (vec2 a, vec2 b) {
return vec2(a.x + b.x, a.y + b.y);
}
vec2 operator - (vec2 a, vec2 b) {
return vec2(a.x - b.x, a.y - b.y);
}
vec2 operator * (float s, vec2 a) {
return vec2(s * a.x, s * a.y);
}
You have a choice between de Casteljau's method, which is to recursively split the control path until you arrive at the point using a linear interpolation, as explained above, or Bezier's method which is to blend the control points.
Bezier's method is
p = (1-t)^3 *P0 + 3*t*(1-t)^2*P1 + 3*t^2*(1-t)*P2 + t^3*P3
for cubics and
p = (1-t)^2 *P0 + 2*(1-t)*t*P1 + t*t*P2
for quadratics.
t is usually on 0-1 but that's not an essential - in fact the curves extend to infinity. P0, P1, etc are the control points. The curve goes through the two end points but not usually through the other points.
Did you use a C# library earlier?
In C++, no standard library function for Bezier curves is available (yet). You can of course roll your own (CodeProject sample) or look for a math library.
This blogpost explains the idea nicely but in Actionscript. Translation should not be much of a problem.
To get an individual point (x, y) along a cubic curve at a given percent of travel (t), with given control points (x1, y1), (x2, y2), (x3, y3), and (x4, y4) I expanded De Casteljau’s algorithm and rearranged the equation to minimize exponents:
//Generalized code, not C++
variables passed to function: t, x1, y1, x2, y2, x3, y3, x4, y4
variables declared in function: t2, t3, x, y
t2 = t * t
t3 = t * t * t
x = t3*x4 + (3*t2 - 3*t3)*x3 + (3*t3 - 6*t2 + 3*t)*x2 + (3*t2 - t3 - 3*t + 1)*x1
y = t3*y4 + (3*t2 - 3*t3)*y3 + (3*t3 - 6*t2 + 3*t)*y2 + (3*t2 - t3 - 3*t + 1)*y1
(t) is a decimal value between 0 and 1 (0 <= t <= 1) that represents percent of travel along the curve.
The formula is the same for x and y, and you can write a function that takes a generic set of 4 control points or group the coefficients together:
t2 = t * t
t3 = t * t * t
A = (3*t2 - 3*t3)
B = (3*t3 - 6*t2 + 3*t)
C = (3*t2 - t3 - 3*t + 1)
x = t3*x4 + A*x3 + B*x2 + C*x1
y = t3*y4 + A*y3 + B*y2 + C*y1
For quadratic functions, a similar approach yields:
t2 = t * t
A = (2*t - 2*t2)
B = (t2 - 2*t + 1)
x = t2*x3 + A*x2 + B*x1
y = t2*y3 + A*y2 + B*y1
If you just want to display a Bezier curve, you can use something like PolyBezier for Windows.
If you want to implement the routine yourself, you can find linear interpolation code all over the Intarnetz.
I believe the Boost libraries have support for this. Linear interpolation, not Beziers specifically. Don't quote me on this, however.
I made an implementation based on this example https://stackoverflow.com/a/11435243/15484522 but for any amount of path points
void bezier(int [] arr, int size, int amount) {
int a[] = new int[size * 2];
for (int i = 0; i < amount; i++) {
for (int j = 0; j < size * 2; j++) a[j] = arr[j];
for (int j = (size - 1) * 2 - 1; j > 0; j -= 2)
for (int k = 0; k <= j; k++)
a[k] = a[k] + ((a[k+2] - a[k]) * i) / amount;
circle(a[0], a[1], 3); // draw a circle, in Processing
}
}
Where arr is array of points {x1, y1, x2, y2, x3, y3... xn, yn}, size is amount of points (twice smaller than array size), and amount is number of output points.
For optimal calculations you can use 2^n amount and bit shift:
void bezier(int [] arr, int size, int dense) {
int a[] = new int[size * 2];
for (int i = 0; i < (1 << dense); i++) {
for (int j = 0; j < size * 2; j++) a[j] = arr[j];
for (int j = (size - 1) * 2 - 1; j > 0; j -= 2)
for (int k = 0; k <= j; k++)
a[k] = a[k] + (((a[k+2] - a[k]) * i) >> dense);
circle(a[0], a[1], 3); // draw a circle, in Processing
}
}
This implementation on github shows how to calculate a simple cubic bezier, with normal and tangent values for values of 't' from 0->1.
It is a direct transposition of the formulas at wikipedia.