I am having a bit of trouble getting the H and G to function properly. What is happening is when I run the program it sometimes finds the best path and sometimes goes way out of the way to get to the location.
Here are some screenshots of what is going on:
Good path find
Bad path find
This is my current setup for F, H and G:
public double f(Node current, Node adj, Node goal)
{
double f = g(current, adj) + h(current, goal);
return f;
}
public double h(Node current, Node goal)
{
double dx = goal.getX() - current.getX();
double dy = goal.getY() - current.getY();
double h = Math.sqrt(dx*dx + dy*dy);
return h;
}
public double g(Node current, Node adj)
{
double dx = adj.getX() - current.getX();
double dy = adj.getY() - current.getY();
double g = Math.sqrt(Math.abs(dx) + Math.abs(dy));
System.out.println("g " + g);
return g;
}
Thanks!
The G value is the cost from the start to the current node and not just to the adjacent node. At the moment you are more doing a greedy search, just going forward in the shortest direction and not looking back how far you have traveled already.
so you got "cost from start to current" + "(under)estimated cost from current to goal".
Related
I'm trying to convert Bezier.js implementation of calculating normals to a Shadertoy program, and the code appears to not use any of the calculated values. It needs to be for quadratic rationals as well.
I found the Javascript code a slight bit hard to follow, so I simplified it for my Shadertoy program:
vec2[3] derive(vec2[3] p)
{
vec2[3] dpoints;
int l_length = 0, j;
for (int i = 2; i > 0; --i) {
vec2[3] l;
for (j = 0; j < i; j++) {
vec2 dpt = vec2(
float(i) * (p[j + 1].x - p[j].x),
float(i) * (p[j + 1].y - p[j].y));
dpoints[l_length] = dpt;
l[l_length] = dpt; ++l_length;
}
p = l;
}
return dpoints;
}
The Bezier.js program continues to add functionality for 3d beziers, in case that has anything to do with rational beziers.
I need to make sense of the rest of the program, since I don't know the theory for calculating the normals.
To spell Pomax's answer out loud:
Only the last calculated value is used, to make a "curve" (line) from origin.
The weights are calculated as w'0 = 2(w1-w0), w'1 = 2(w2-w1).
The resulting bezier at t gives the tangent of the original bezier at t.
I hope I got this right I haven't tried this yet.
I am trying to figure out where a bunch of line-segments clip into a window around them. I saw the Liang–Barsky algorithm, but that seems to assume the segments already clip the edges of the window, which these do not.
Say I have a window from (0,0) to (26,16), and the following segments:
(7,6) - (16,3)
(10,6) - (19,6)
(13,10) - (21,3)
(16,12) - (19,14)
Illustration:
I imagine I need to extend the segments to a certain X or Y point, till they hit the edge of the window, but I don't know how.
How would I find the points where these segments (converted to lines?) clip into the edge of the window? I will be implementing this in C#, but this is pretty language-agnostic.
If you have two line segments P and Q with points
P0 - P1
Q0 - Q1
The line equations are
P = P0 + t(P1 - P0)
Q = Q0 + r(Q1 - Q0)
then to find out where they intersect after extension you need to solve the following equation for t and r
P0 + t(P1 - P0) = Q0 + r(Q1 - Q0)
The following code can do this. ( Extracted from my own code base )
public static (double t, double r )? SolveIntersect(this Segment2D P, Segment2D Q)
{
// a-d are the entries of a 2x2 matrix
var a = P.P1.X - P.P0.X;
var b = -Q.P1.X + Q.P0.X;
var c = P.P1.Y - P.P0.Y;
var d = -Q.P1.Y + Q.P0.Y;
var det = a*d - b*c;
if (Math.Abs( det ) < Utility.ZERO_TOLERANCE)
return null;
var x = Q.P0.X - P.P0.X;
var y = Q.P0.Y - P.P0.Y;
var t = 1/det*(d*x - b*y);
var r = 1/det*(-c*x + a*y);
return (t, r);
}
If null is returned from the function then it means the lines are parallel and cannot intersect. If a result is returned then you can do.
var result = SolveIntersect( P, Q );
if (result != null)
{
var ( t, r) = result.Value;
var p = P.P0 + t * (P.P1 - P.P0);
var q = Q.P0 + t * (Q.P1 - Q.P0);
// p and q are the same point of course
}
The extended line segments will generally intersect more than one box edge but only one of those intersections will be inside the box. You can check this easily.
bool IsInBox(Point corner0, Point corner1, Point test) =>
(test.X > corner0.X && test.X < corner1.X && test.Y > corner0.Y && test.Y < corner1.Y ;
That should give you all you need to extend you lines to the edge of your box.
I managed to figure this out.
I can extend my lines to the edge of the box by first finding the equations of my lines, then solving for the X and Y of each of the sides to get their corresponding point. This requires passing the max and min Y and the max and min X into the following functions, returning 4 values. If the point is outside the bounds of the box, it can be ignored.
My code is in C#, and is making extension methods for EMGU's LineSegment2D. This is a .NET wrapper for OpenCv.
My Code:
public static float GetYIntersection(this LineSegment2D line, float x)
{
Point p1 = line.P1;
Point p2 = line.P2;
float dx = p2.X - p1.X;
if(dx == 0)
{
return float.NaN;
}
float m = (p2.Y - p1.Y) / dx; //Slope
float b = p1.Y - (m * p1.X); //Y-Intercept
return m * x + b;
}
public static float GetXIntersection(this LineSegment2D line, float y)
{
Point p1 = line.P1;
Point p2 = line.P2;
float dx = p2.X - p1.X;
if (dx == 0)
{
return float.NaN;
}
float m = (p2.Y - p1.Y) / dx; //Slope
float b = p1.Y - (m * p1.X); //Y-Intercept
return (y - b) / m;
}
I can then take these points, check if they are in the bounds of the box, discard the ones that are not, remove duplicate points (line goes directly into corner). This will leave me with one x and one y value, which I can then pair to the corresponding min or max Y or X values I passed into the functions to make 2 points. I can then make my new segment with the two points.
Wiki description of Liang-Barsky algorithm is not bad, but code is flaw.
Note: this algorithm intended to throw out lines without intersection as soon as possible. If most of lines intersect the rectangle, then approach from your answer might be rather effective, otherwise L-B algorithm wins.
This page describes approach in details and contains concise effective code:
// Liang-Barsky function by Daniel White # http://www.skytopia.com/project/articles/compsci/clipping.html
// This function inputs 8 numbers, and outputs 4 new numbers (plus a boolean value to say whether the clipped line is drawn at all).
//
bool LiangBarsky (double edgeLeft, double edgeRight, double edgeBottom, double edgeTop, // Define the x/y clipping values for the border.
double x0src, double y0src, double x1src, double y1src, // Define the start and end points of the line.
double &x0clip, double &y0clip, double &x1clip, double &y1clip) // The output values, so declare these outside.
{
double t0 = 0.0; double t1 = 1.0;
double xdelta = x1src-x0src;
double ydelta = y1src-y0src;
double p,q,r;
for(int edge=0; edge<4; edge++) { // Traverse through left, right, bottom, top edges.
if (edge==0) { p = -xdelta; q = -(edgeLeft-x0src); }
if (edge==1) { p = xdelta; q = (edgeRight-x0src); }
if (edge==2) { p = -ydelta; q = -(edgeBottom-y0src);}
if (edge==3) { p = ydelta; q = (edgeTop-y0src); }
if(p==0 && q<0) return false; // Don't draw line at all. (parallel line outside)
r = q/p;
if(p<0) {
if(r>t1) return false; // Don't draw line at all.
else if(r>t0) t0=r; // Line is clipped!
} else if(p>0) {
if(r<t0) return false; // Don't draw line at all.
else if(r<t1) t1=r; // Line is clipped!
}
}
x0clip = x0src + t0*xdelta;
y0clip = y0src + t0*ydelta;
x1clip = x0src + t1*xdelta;
y1clip = y0src + t1*ydelta;
return true; // (clipped) line is drawn
}
I have a vector _pts that contains values for (x,y,z), i.e. a point in 3D. Starting with _pts[0], I want to select those points whose distance between previously selected points is bigger than sampleRadius.
Here is my code but apparently something's wrong because is selecting a lot of points instead of just selecting a few.
Can anyone see what am I doing wrong? Probably you would need more code to see what can be missed, but I would also appreciate any idea on how can I implement this.
float distance;
bool distanceIsOk;
//PICK A POINT IN VECTOR _pts
for (int cPIdx = 0; cPIdx < _pts.size(); cPIdx++) {
distanceIsOk = true;
//CHECK DISTANCE AGAINST PREVIOUSLY PICKED POINTS
for (int dPIdx = 0; dPIdx < indeces.size(); dPIdx++) {
distance = sqrt(
(_pts[cPIdx].v[0] - _pts[dPIdx].v[0])*(_pts[cPIdx].v[0] - _pts[dPIdx].v[0]) +
(_pts[cPIdx].v[1] - _pts[dPIdx].v[1])*(_pts[cPIdx].v[1] - _pts[dPIdx].v[1]) +
(_pts[cPIdx].v[2] - _pts[dPIdx].v[2])*(_pts[cPIdx].v[2] - _pts[dPIdx].v[2])
);
//IF DISTANCE IS <= SUBSAMPLERADIUS FOR AT LEAST ONE PREVIOUSLY SELECTED POINT
if (distance <= subsampleRadius) {
//DISCARD THE POINT
distanceIsOk = false;
dPIdx += indeces.size();
}
}
//OTHERWISE INCLUDE THAT POINT
if (distanceIsOk == true) {
indeces.push_back(cPIdx);
}
}
Found the error. Instead of
distance = sqrt(
(_pts[cPIdx].v[0] - _pts[dPIdx].v[0])*(_pts[cPIdx].v[0] - _pts[dPIdx].v[0]) +
(_pts[cPIdx].v[1] - _pts[dPIdx].v[1])*(_pts[cPIdx].v[1] - _pts[dPIdx].v[1]) +
(_pts[cPIdx].v[2] - _pts[dPIdx].v[2])*(_pts[cPIdx].v[2] - _pts[dPIdx].v[2])
it should be
distance = sqrt(
(_pts[cPIdx].v[0] - _pts[indeces[dPIdx]].v[0])*(_pts[cPIdx].v[0] - _pts[indeces[dPIdx]].v[0]) +
(_pts[cPIdx].v[1] - _pts[indeces[dPIdx]].v[1])*(_pts[cPIdx].v[1] - _pts[indeces[dPIdx]].v[1]) +
(_pts[cPIdx].v[2] - _pts[indeces[dPIdx]].v[2])*(_pts[cPIdx].v[2] - _pts[indeces[dPIdx]].v[2])
I am experiment kinect on winrt for metro app.
I am trying to obtain angle at the elbow.
normally i will do the following
Vector3D handLeftVector = new Vector3D(HandLeftX, HandLeftY, HandLeftZ);
handLeftVector.Normalize();
Vector3D ElbowLeftEVector = new Vector3D(ElbowLeftX, ElbowLeftY, ElbowLeftZ);
ElbowLeftEVector.Normalize();
Vector3D ShoulderLeftVector = new Vector3D(ShoulderLeftX, ShoulderLeftY, ShoulderLeftZ);
ShoulderLeftVector.Normalize();
Vector3D leftElbowV1 = ShoulderLeftVector - ElbowLeftEVector;
Vector3D leftElbowV2 = handLeftVector - ElbowLeftEVector;
double leftElbowAngle = Vector3D.AngleBetween(leftElbowV1, leftElbowV2);
However Vector3D object isn't available in winrt.
I had decided to replicate the Vector3D method as below. However the result doesn't seem to be as expected. Did I make a mistake anywhere?
double leftElbowV1X = ShoulderLeftX - ElbowLeftX;
double leftElbowV1Y = ShoulderLeftY - ElbowLeftY;
double leftElbowV1Z = ShoulderLeftZ - ElbowLeftZ;
double leftElbowV2X = handLeftX - ElbowLeftX;
double leftElbowV2Y = handLeftY - ElbowLeftY;
double leftElbowV2Z = handLeftZ - ElbowLeftZ;
double product = leftElbowV1X * leftElbowV2X + leftElbowV1Y * leftElbowV2Y + leftElbowV1Z * leftElbowV2Z;
double magnitudeA = Math.Sqrt(Math.Pow(leftElbowV1X, 2) + Math.Pow(leftElbowV1Y, 2) + Math.Pow(leftElbowV1Z, 2));
double magnitudeB = Math.Sqrt(Math.Pow(leftElbowV2X, 2) + Math.Pow(leftElbowV2Y, 2) + Math.Pow(leftElbowV2Z, 2));
magnitudeA = Math.Abs(magnitudeA);
magnitudeB = Math.Abs(magnitudeB);
double cosDelta = product / (magnitudeA * magnitudeB);
double angle = Math.Acos(cosDelta) *180.0 / Math.P;
And is there a need to normalize it?
i had managed to resolve it, however i am thinking if there is a more efficient way of doing.
Not sure if this helps but this is some old angle code I use, return in degrees:
float AngleBetween(Vector3 from, Vector3 dest) {
float len = from.magnitude * dest.magnitude;
if(len < Mathf.Epsilon) len = Mathf.Epsilon;
float f = Vector3.Dot(from,dest) / len;
if(f>1.0f)f=1.0f;
else if ( f < -1.0f) f = -1.0f;
return Mathf.Acos(f) * 180.0f / (float)Math.PI;
}
It's obviously using API specific syntax but I think the method is clear.
To implement a 2D animation I am looking for interpolating values between two key frames with the velocity of change defined by a Bezier curve. The problem is Bezier curve is represented in parametric form whereas requirement is to be able to evaluate the value for a particular time.
To elaborate, lets say the value of 10 and 40 is to be interpolated across 4 seconds with the value changing not constantly but as defined by a bezier curve represented as 0,0 0.2,0.3 0.5,0.5 1,1.
Now if I am drawing at 24 frames per second, I need to evaluate the value for every frame. How can I do this ? I looked at De Casteljau algorithm and thought that dividing the curve into 24*4 pieces for 4 seconds would solve my problem but that sounds erroneous as time is along the "x" axis and not along the curve.
To further simplify
If I draw the curve in a plane, the x axis represents the time and the y axis the value I am looking for. What I actually require is to to be able to find out "y" corresponding to "x". Then I can divide x in 24 divisions and know the value for each frame
I was facing the same problem: Every animation package out there seems to use Bézier curves to control values over time, but there is no information out there on how to implement a Bézier curve as a y(x) function. So here is what I came up with.
A standard cubic Bézier curve in 2D space can be defined by the four points P0=(x0, y0) .. P3=(x3, y3).
P0 and P3 are the end points of the curve, while P1 and P2 are the handles affecting its shape. Using a parameter t ϵ [0, 1], the x and y coordinates for any given point along the curve can then be determined using the equations
A) x = (1-t)3x0 + 3t(1-t)2x1 + 3t2(1-t)x2 + t3x3 and
B) y = (1-t)3y0 + 3t(1-t)2y1 + 3t2(1-t)y2 + t3y3.
What we want is a function y(x) that, given an x coordinate, will return the corresponding y coordinate of the curve. For this to work, the curve must move monotonically from left to right, so that it doesn't occupy the same x coordinate more than once on different y positions. The easiest way to ensure this is to restrict the input points so that x0 < x3 and x1, x2 ϵ [x0, x3]. In other words, P0 must be to the left of P3 with the two handles between them.
In order to calculate y for a given x, we must first determine t from x. Getting y from t is then a simple matter of applying t to equation B.
I see two ways of determining t for a given y.
First, you might try a binary search for t. Start with a lower bound of 0 and an upper bound of 1 and calculate x for these values for t via equation A. Keep bisecting the interval until you get a reasonably close approximation. While this should work fine, it will neither be particularly fast nor very precise (at least not both at once).
The second approach is to actually solve equation A for t. That's a bit tough to implement because the equation is cubic. On the other hand, calculation becomes really fast and yields precise results.
Equation A can be rewritten as
(-x0+3x1-3x2+x3)t3 + (3x0-6x1+3x2)t2 + (-3x0+3x1)t + (x0-x) = 0.
Inserting your actual values for x0..x3, we get a cubic equation of the form at3 + bt2 + c*t + d = 0 for which we know there is only one solution within [0, 1]. We can now solve this equation using an algorithm like the one posted in this Stack Overflow answer.
The following is a little C# class demonstrating this approach. It should be simple enough to convert it to a language of your choice.
using System;
public class Point {
public Point(double x, double y) {
X = x;
Y = y;
}
public double X { get; private set; }
public double Y { get; private set; }
}
public class BezierCurve {
public BezierCurve(Point p0, Point p1, Point p2, Point p3) {
P0 = p0;
P1 = p1;
P2 = p2;
P3 = p3;
}
public Point P0 { get; private set; }
public Point P1 { get; private set; }
public Point P2 { get; private set; }
public Point P3 { get; private set; }
public double? GetY(double x) {
// Determine t
double t;
if (x == P0.X) {
// Handle corner cases explicitly to prevent rounding errors
t = 0;
} else if (x == P3.X) {
t = 1;
} else {
// Calculate t
double a = -P0.X + 3 * P1.X - 3 * P2.X + P3.X;
double b = 3 * P0.X - 6 * P1.X + 3 * P2.X;
double c = -3 * P0.X + 3 * P1.X;
double d = P0.X - x;
double? tTemp = SolveCubic(a, b, c, d);
if (tTemp == null) return null;
t = tTemp.Value;
}
// Calculate y from t
return Cubed(1 - t) * P0.Y
+ 3 * t * Squared(1 - t) * P1.Y
+ 3 * Squared(t) * (1 - t) * P2.Y
+ Cubed(t) * P3.Y;
}
// Solves the equation ax³+bx²+cx+d = 0 for x ϵ ℝ
// and returns the first result in [0, 1] or null.
private static double? SolveCubic(double a, double b, double c, double d) {
if (a == 0) return SolveQuadratic(b, c, d);
if (d == 0) return 0;
b /= a;
c /= a;
d /= a;
double q = (3.0 * c - Squared(b)) / 9.0;
double r = (-27.0 * d + b * (9.0 * c - 2.0 * Squared(b))) / 54.0;
double disc = Cubed(q) + Squared(r);
double term1 = b / 3.0;
if (disc > 0) {
double s = r + Math.Sqrt(disc);
s = (s < 0) ? -CubicRoot(-s) : CubicRoot(s);
double t = r - Math.Sqrt(disc);
t = (t < 0) ? -CubicRoot(-t) : CubicRoot(t);
double result = -term1 + s + t;
if (result >= 0 && result <= 1) return result;
} else if (disc == 0) {
double r13 = (r < 0) ? -CubicRoot(-r) : CubicRoot(r);
double result = -term1 + 2.0 * r13;
if (result >= 0 && result <= 1) return result;
result = -(r13 + term1);
if (result >= 0 && result <= 1) return result;
} else {
q = -q;
double dum1 = q * q * q;
dum1 = Math.Acos(r / Math.Sqrt(dum1));
double r13 = 2.0 * Math.Sqrt(q);
double result = -term1 + r13 * Math.Cos(dum1 / 3.0);
if (result >= 0 && result <= 1) return result;
result = -term1 + r13 * Math.Cos((dum1 + 2.0 * Math.PI) / 3.0);
if (result >= 0 && result <= 1) return result;
result = -term1 + r13 * Math.Cos((dum1 + 4.0 * Math.PI) / 3.0);
if (result >= 0 && result <= 1) return result;
}
return null;
}
// Solves the equation ax² + bx + c = 0 for x ϵ ℝ
// and returns the first result in [0, 1] or null.
private static double? SolveQuadratic(double a, double b, double c) {
double result = (-b + Math.Sqrt(Squared(b) - 4 * a * c)) / (2 * a);
if (result >= 0 && result <= 1) return result;
result = (-b - Math.Sqrt(Squared(b) - 4 * a * c)) / (2 * a);
if (result >= 0 && result <= 1) return result;
return null;
}
private static double Squared(double f) { return f * f; }
private static double Cubed(double f) { return f * f * f; }
private static double CubicRoot(double f) { return Math.Pow(f, 1.0 / 3.0); }
}
You have a few options:
Let's say your curve function F(t) takes a parameter t that ranges from 0 to 1 where F(0) is the beginning of the curve and F(1) is the end of the curve.
You could animate motion along the curve by incrementing t at a constant change per unit of time.
So t is defined by function T(time) = Constant*time
For example, if your frame is 1/24th of a second, and you want to move along the curve at a rate of 0.1 units of t per second, then each frame you increment t by 0.1 (t/s) * 1/24 (sec/frame).
A drawback here is that your actual speed or distance traveled per unit time will not be constant. It will depends on the positions of your control points.
If you want to scale speed along the curve uniformly you can modify the constant change in t per unit time. However, if you want speeds to vary dramatically you will find it difficult to control the shape of the curve. If you want the velocity at one endpoint to be much larger, you must move the control point further away, which in turn pulls the shape of the curve towards that point. If this is a problem, you may consider using a non constant function for t. There are a variety of approaches with different trade-offs, and we need to know more details about your problem to suggest a solution. For example, in the past I have allowed users to define the speed at each keyframe and used a lookup table to translate from time to parameter t such that there is a linear change in speed between keyframe speeds (it's complicated).
Another common hangup: If you are animating by connecting several Bezier curves, and you want the velocity to be continuous when moving between curves, then you will need to constrain your control points so they are symmetrical with the adjacent curve. Catmull-Rom splines are a common approach.
I've answered a similar question here. Basically if you know the control points before hand then you can transform the f(t) function into a y(x) function. To not have to do it all by hand you can use services like Wolfram Alpha to help you with the math.