http://pastebin.com/ccck8LjE
Hello i've written this code to try and simulate the moon earth sun system. I get a circular orbit for the earth but the moon goes mad.
Is there a better way to try and do this whilst still using a euler/leapfrog method?
I didn't take the effort to carefully review your code or test its behavior, but it is well known that the Euler method gains energy in such simulations, so "trapezoidal" is generally used instead.
https://en.wikipedia.org/wiki/Trapezoidal_rule
If I understand correctly, your method is second order in the relationship between position and velocity (I assume with the intent of avoiding that energy gain) but it appears to be first order in the relationship between acceleration and velocity.
You might be able to keep the leapfrog design (rather than trapezoidal) but in trying to do so, you should do a better job of making the extra state explicit to eliminate the use of second derivative.
Edit: I reread the Wikipedia page on leapfrog and reread your code and it is clear you are not using leapfrog (as I said earlier because you only apply it to position\velocity not to velocity\acceleration). Note that page also shows you how to simplify the equations of leapfrog for the situation (that you have) in which acceleration depends on position and not on velocity:
https://en.wikipedia.org/wiki/Leapfrog_integration
Maybe you could try to use Runge Kutta methods if you want to keep using explicit integration : https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
RK4 is usually enough.
Related
We're currently using Matlab's fmincon function to do non-linear optimization for a project I'm working on. We need to port that part of the project to C++ in order to integrate it with other parts of the project. Is there a good way to compile the fmincon function into a library that we can use in C++? Or, is there already a library available somewhere that implements fmincon?
If neither of the above are an option, what optimization libraries are available that would be fairly easy to switch to from fmincon?
Background info:
We're trying to optimize a waypoint flight path of a UAV to follow a given waypoint camera path along the ground as closely as possible. The waypoints between the two paths correspond temporally, so the camera gimbal will be pointed at the i-th camera waypoint when the UAV arrives at the i-th flight path waypoint. The flight path segments will all be the same length since the UAV flies at a constant speed. The turn radius is also constrained by an upper bound. There are no constraints on the camera path, so its segments may be longer or shorter than the flight path segments and it may have sharp turns. The cost function is the sum-squared distance between corresponding flight waypoints and camera waypoints (ignoring altitude differences).
Most of the time, libraries out there won't try to be a black box magic one-size-fits-all optimization tool like fmincon is- instead they'll require you to provide more detail and make more choices on your own, which is simpler for them and should result in your software being faster. You can certainly use the MATLAB Engine or MATLAB Compiler to call fmincon from your program, but most likely your software will run a good deal faster (and you can avoid purchasing the MATLAB Compiler) if you can use a little more knowledge about the structure your optimization problem has and call an appropriate algorithm.
Your background info doesn't describe what you're doing - esp. what your feasible set is- clearly enough for me to be able to tell you what to use, so all I can do is point you in the direction of relevant resources.
Wikipedia's page on optimization links to lists of optimization software- most importantly, it describes more specific kinds of optimization problems (for instance, can you formulate your problem as quadratic programming with linear constraints?) and software appropriate for each situation.
Boyd's book on convex optimization and the linked course materials & videos are really good resources.
Because this is a complex question that usually results in much confusion (I've asked variations on this question previously, but never asked the question the right way and never got an answer), I'm going to try to make this as clear as possible.
Facts:
I'm using Unity.
I'm can get the Forward, Up and Right vectors easily from any Quaternion rotation.
I can't simply record my own Euler angles, modify them and apply the rotation through a new Quaternion because the object is controlled by physics.
I don't understand maths very well at all unless it's written in code (or pseudo-code), so this would be most beneficial to me.
A C++ style answer would be easiest for me to understand, but I can work out pretty much any kind of code.
I'm NOT trying to get anyone to write the code for me! I'm only asking for the answer in code or pseudo-code because I never learned to read normal maths squiggles; I'm a programmer, not a mathematician.
Unity uses a left-handed coordinate system. X = right, Y = up, Z = forward.
What I'm trying to do:
I'm trying to play an animation on a humanoid bone structure and, using torque (rotational force), push the physics ragdoll into approximately the same pose as the bone structure.
The problem:
I can work fully in Quaternions right up to the point where I need to apply the torque to the rigidbodies. The AddTorque function effectively works in Euler angles, which means I can't use the Quaternions. I can easily extract Euler angles from the Quaternions, but they are unreliable and cause the ragdoll to spaz out severely.
What I need:
I need to calculate reliable 3D Euler angles (as in, ones that don't flip from + to - "randomly") from Forward, Up and Right vectors. I realise this is a bit complicated, but that's why I ask here: I lack the knowledge and experience to work out this problem myself.
Given that the vectors themselves are reliable, I see no reason why it would not be possible to work out reliable Euler angles from them. Also, I don't know what order of rotation I would want or need for the Euler angles, but I believe that would be fairly easy to modify later.
Any help would be much appreciated!
First, I'd like to say that I solved my problem entirely due to #Tobias's efforts. Many, many thanks! All this time I've been approaching the problem from the wrong end. I assumed that I needed to use AddTorque in a particular way (with Euler angles) and worked from there, but #Tobias (and #JarkkoL a little later) pointed out that I needed to use AddTorque differently.
So this is what I did, in UnityScript (effectively JavaScript):
var quat0:Quaternion;
var quat1:Quaternion;
var quat10:Quaternion;
quat0=transform.rotation;
quat1=target.transform.rotation;
quat10=quat1*Quaternion.Inverse(quat0);
rigidbody.AddTorque(quat10.x,quat10.y,quat10.z,ForceMode.Force);
And, against all expectations, this WORKS!! It just... works! Sure, it takes a long time for the rigidbody cube to settle down, but that's because I need a PID controller. Or maybe quat10 needs normalising, not sure. I'll work it out :)
I had no idea you could actually use that part of a quat by itself.
First off, I think you would have better luck in Unity forums for Unity specific questions (: That said, I think you are misinterpreting the AddTorque() interface if this one is what you are using: http://docs.unity3d.com/ScriptReference/Rigidbody.AddTorque.html
Instead of passing Euler angles you pass a vector to the function that's the axis of rotation. I'm not familiar with Unity, but I believe the length of the vector specifies how much torque to add. Euler angles are inherently bad representation for rotations so you should always assume (unless otherwise documented) that well established API's work with quaternions, axis/angle pairs or matrices when it comes to rotations. Euler angles are more of a convenience representation for end users.
I'm trying to make a game or 3D application using openGL. The game/program will have many objects in them and drawn to the screen(around 7000 of them). When I render them, I would need to calculate the distance between the camera and the object and sort them in order to correctly render the objects within the scene. Knowing this, what is the best way to sort them? I really want the sorting to be done really fast, but I've heard there are "trade off" for them, so what algorithm should I use to get the best performance out of it?
Any help would be greatly appreciated.
Edit: a lot of people are talking about the z-buffer/depth buffer. This doesn't work in some cases like a few people talked about. This is why I asked this question.
Sorting by distance doesn't solve the transparency problem perfectly. Consider the situation where two transparent surfaces intersect and each has a part which is closer to you. Perhaps rare in games, but still something to consider if you don't want an occasional glitched look to your renderer.
The better solution is order-independent transparency. With the latest graphics hardware supporting atomic operations, you can use an A-buffer to do this with little memory overhead and in a single pass so it is pretty efficient. See for example this article.
The issue of sorting your scene is still a valid one, though, even if it isn't for transparency -- it is still useful to sort opaque objects front to back to to allow depth testing to discard unseen fragments. For this, Vaughn provided the great solution of BSP trees -- these have been used for this purpose for as long as 3D games have been around.
Use http://en.wikipedia.org/wiki/Insertion_sort which has O(n) complexity for nearly sorted arrrays.
In your case by exploiting temporal cohesion insertion sort gives fastest results.
It is used for http://en.wikipedia.org/wiki/Sweep_and_prune
From link above:
In many applications, the configuration of physical bodies from one time step to the next changes very little. Many of the objects may not move at all. Algorithms have been designed so that the calculations done in a preceding time step can be reused in the current time step, resulting in faster completion of the calculation.
So in such cases insertion sort is best(or similar sorts with O(n) at best case)
So I have an iterative closest point (ICP) algorithm that has been written and will fit a model to a point cloud. As a quick tutorial for those not in the know ICP is a simple algorithm that fits points to a model ultimately providing a homogeneous transform matrix between the model and points.
Here is a quick picture tutorial.
Step 1. Find the closest point in the model set to your data set:
Step 2: Using a bunch of fun maths (sometimes based on gradiant descent or SVD) pull the clouds closer together and repeat untill a pose is formed:
![Figure 2][2]
Now that bit is simple and working, what i would like help with is:
How do I tell if the pose that I have is a good one?
So currently I have two ideas, but they are kind of hacky:
How many points are in the ICP Algorithm. Ie, if I am fitting to almost no points, I assume that the pose will be bad:
But what if the pose is actually good? It could be, even with few points. I dont want to reject good poses:
So what we see here is that low points can actually make a very good position if they are in the right place.
So the other metric investigated was the ratio of the supplied points to the used points. Here's an example
Now we exlude points that are too far away because they will be outliers, now this means we need a good starting position for the ICP to work, but i am ok with that. Now in the above example the assurance will say NO, this is a bad pose, and it would be right because the ratio of points vs points included is:
2/11 < SOME_THRESHOLD
So thats good, but it will fail in the case shown above where the triangle is upside down. It will say that the upside down triangle is good because all of the points are used by ICP.
You don't need to be an expert on ICP to answer this question, i am looking for good ideas. Using knowledge of the points how can we classify whether it is a good pose solution or not?
Using both of these solutions together in tandem is a good suggestion but its a pretty lame solution if you ask me, very dumb to just threshold it.
What are some good ideas for how to do this?
PS. If you want to add some code, please go for it. I am working in c++.
PPS. Someone help me with tagging this question I am not sure where it should fall.
One possible approach might be comparing poses by their shapes and their orientation.
Shapes comparison can be done with Hausdorff distance up to isometry, that is poses are of the same shape if
d(I(actual_pose), calculated_pose) < d_threshold
where d_threshold should be found from experiments. As isometric modifications of X I would consider rotations by different angles - seems to be sufficient in this case.
Is poses have the same shape, we should compare their orientation. To compare orientation we could use somewhat simplified Freksa model. For each pose we should calculate values
{x_y min, x_y max, x_z min, x_z max, y_z min, y_z max}
and then make sure that each difference between corresponding values for poses does not break another_threshold, derived from experiments as well.
Hopefully this makes some sense, or at least you can draw something useful for your purpose from this.
ICP attempts to minimize the distance between your point-cloud and a model, yes? Wouldn't it make the most sense to evaluate it based on what that distance actually is after execution?
I'm assuming it tries to minimize the sum of squared distances between each point you try to fit and the closest model point. So if you want a metric for quality, why not just normalize that sum, dividing by the number of points it's fitting. Yes, outliers will disrupt it somewhat but they're also going to disrupt your fit somewhat.
It seems like any calculation you can come up with that provides more insight than whatever ICP is minimizing would be more useful incorporated into the algorithm itself, so it can minimize that too. =)
Update
I think I didn't quite understand the algorithm. It seems that it iteratively selects a subset of points, transforms them to minimize error, and then repeats those two steps? In that case your ideal solution selects as many points as possible while keeping error as small as possible.
You said combining the two terms seemed like a weak solution, but it sounds to me like an exact description of what you want, and it captures the two major features of the algorithm (yes?). Evaluating using something like error + B * (selected / total) seems spiritually similar to how regularization is used to address the overfitting problem with gradient descent (and similar) ML algorithms. Selecting a good value for B would take some experimentation.
Looking at your examples, it seems that one of the things that determines whether the match is good or not, is the quality of the points. Could you use/calculate a weighting factor in calculating your metric?
For example, you could weight down points which are co-linear / co-planar, or spatially close, as they probably define the same feature. That would perhaps allow your upside-down triangle to be rejected (as the points are in a line, and that not a great indicator of the overall pose) but the corner-case would be ok, as they roughly define the hull.
Alternatively, maybe the weighting should be on how distributed the points are around the pose, again trying to ensure you have good coverage, rather than matching small indistinct features.
Is there a pathfinding algorithm also suited for real 3D environments e.g. real Buildings with multiple stairs etc. A C++ library or open implementation would be splendid ;-)
One solution I saw was Djikstra but I wonder whether there is something more optimal.
Normal A* would not work better then Djikstra since the distance heuristic does not work well (Position one floor above destination).
Another solution that I'm currently pondering is the mapping of the 3d environment on a 2d graph. So if there is some available C++ implementation/library going this way it would be helpful too.
If the path has to take into account the ability to navigate through obstacles (i.e. the movement is that of some entity with known volume in space), then I'd recommend looking into the literature on robot motion planning. The notion of a configuration space allows you to handle changes in pose in order to deal with obstacles. See the classic textbook by Jean-Claude Latombe
For simpler scenarios, you can probably make do with path planning algorithms used in first person computer games, which are similar to Dijkstra, A* (example)
For an approximation algorithm you can easily map the 3d to a 1d curve and traverse an octree with a gray code. That way you can reorder each path. I don't know if there is a guarantee to be within the optimum solution but it must be better then any heuristic method.