I am trying to make boids algorithm in Unity 3D.
I got into one problem: How to implement field of view in specific angle?
360 degrees is easy - U check only distance between two boids. But I dont want boids able to look behind themself. I also want to be able to change angle of view in Inspector, so it must be based on calculations.
I would be gratefull for any ideas:(
I already tried with mesh collider which is cone but it didnt go well. - not working for 180 and higher. So I am looking best way to calculate this.
Assume that the boid is at point p = (p.x, p.y, p.z) heading toward some point h = (h.x, h.y, h.z), and we want to know whether an object at point q = (q.x, q.y, q.z) is in the boid's field of vision.
The Law of Cosines gives us a formula for the cosine of the angle φ between the boid's heading and the boid's path to the object:
(h−p) · (q−p)
cos(φ) = ---------------
||h−p|| ||q−p||
= (dx1*dx2 + dy1*dy2 + dz1*dz2) /
(sqrt(dx1*dx1 + dy1*dy1 + dz1*dz1) * sqrt(dx2*dx2 + dy2*dy2 + dz2*dz2))
where
dx1 = h.x - p.x
dy1 = h.y - p.y
dz1 = h.z - p.z
dx2 = q.x - p.x
dy2 = q.y - p.y
dz2 = q.z - p.z
Given some angle ρ (in whatever units your cosine function accepts, usually radians) past which the boid cannot see (putting the field of vision at 2ρ), we have
φ > ρ if and only if cos(φ) < cos(ρ),
so we can precompute cos(ρ) and then use the above formula for repeated tests.
To avoid division by zero and other numerical problems, you might want to check whether the denominator of the division is very small and if so declare that the boid can feel whatever the object is even outside its field of vision.
Related
I can't quite figure this one out.
I am trying to approximate the location (latitude / longitude) of a beacon based on 3 distance measurements from 3 fixed locations. However the distance readings available may have an error of up to 1 km.
Similar questions regarding trilateration have been asked here (with precise measurements), here, here (distance measurement errors in Java, but not in lat/lon coordinates and no answers) as well as others. I also managed to dig up this paper dealing with imperfect measurement data, however it for one assumes a cartesian coordinate system and is also rather mathematical than close to a usable implementation.
So none of above links and answers are really applicable to the following problem:
All available distance measurements are approximated in km (where data most frequently contains readings in-between 1 km and 100 km, in case this matters)
Measurement errors of up to 1 km are possible.
3 distance measurements are performed based on 3 fixed (latitude / longitude known) positions.
target approximation should also be a latitude / longitude combination.
So far I have adapted this Answer to C#, however I noticed that due to the measurement inaccuracies this algorithm does not work (as the algorithm assumes the 3 distance-circles to perfectly intersect with each other):
public static class Trilateration
{
public static GeoLocation Compute(DistanceReading point1, DistanceReading point2, DistanceReading point3)
{
// not my code :P
// assuming elevation = 0
const double earthR = 6371d;
//using authalic sphere
//if using an ellipsoid this step is slightly different
//Convert geodetic Lat/Long to ECEF xyz
// 1. Convert Lat/Long to radians
// 2d. Convert Lat/Long(radians) to ECEF
double xA = earthR * (Math.Cos(Radians(point1.GeoLocation.Latitude)) * Math.Cos(Radians(point1.GeoLocation.Longitude)));
double yA = earthR * (Math.Cos(Radians(point1.GeoLocation.Latitude)) * Math.Sin(Radians(point1.GeoLocation.Longitude)));
double zA = earthR * Math.Sin(Radians(point1.GeoLocation.Latitude));
double xB = earthR * (Math.Cos(Radians(point2.GeoLocation.Latitude)) * Math.Cos(Radians(point2.GeoLocation.Longitude)));
double yB = earthR * (Math.Cos(Radians(point2.GeoLocation.Latitude)) * Math.Sin(Radians(point2.GeoLocation.Longitude)));
double zB = earthR * (Math.Sin(Radians(point2.GeoLocation.Latitude)));
double xC = earthR * (Math.Cos(Radians(point3.GeoLocation.Latitude)) * Math.Cos(Radians(point3.GeoLocation.Longitude)));
double yC = earthR * (Math.Cos(Radians(point3.GeoLocation.Latitude)) * Math.Sin(Radians(point3.GeoLocation.Longitude)));
double zC = earthR * Math.Sin(Radians(point3.GeoLocation.Latitude));
// a 64 bit Vector3 implementation :)
Vector3_64 P1 = new(xA, yA, zA);
Vector3_64 P2 = new(xB, yB, zB);
Vector3_64 P3 = new(xC, yC, zC);
//from wikipedia
//transform to get circle 1 at origin
//ransform to get circle 2d on x axis
Vector3_64 ex = (P2 - P1).Normalize();
double i = Vector3_64.Dot(ex, P3 - P1);
Vector3_64 ey = (P3 - P1 - i * ex).Normalize();
Vector3_64 ez = Vector3_64.Cross(ex, ey);
double d = (P2 - P1).Length;
double j = Vector3_64.Dot(ey, P3 - P1);
//from wikipedia
//plug and chug using above values
double x = (Math.Pow(point1.DistanceKm, 2d) - Math.Pow(point2.DistanceKm, 2d) + Math.Pow(d, 2d)) / (2d * d);
double y = ((Math.Pow(point1.DistanceKm, 2d) - Math.Pow(point3.DistanceKm, 2d) + Math.Pow(i, 2d) + Math.Pow(j, 2d)) / (2d * j)) - ((i / j) * x);
// only one case shown here
double z = Math.Sqrt(Math.Pow(point1.DistanceKm, 2d) - Math.Pow(x, 2d) - Math.Pow(y, 2d));
//triPt is a vector with ECEF x,y,z of trilateration point
Vector3_64 triPt = P1 + x * ex + y * ey + z * ez;
//convert back to lat/long from ECEF
//convert to degrees
double lat = Degrees(Math.Asin(triPt.Z / earthR));
double lon = Degrees(Math.Atan2(triPt.Y, triPt.X));
return new GeoLocation(lat, lon);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private static double Radians(double degrees) =>
degrees * Math.Tau / 360d;
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private static double Degrees(double radians) =>
radians * 360d / Math.Tau;
}
Above code most often than not does not work in my case, and instead only returns "Not a number" as it tries to take the square root of a negative number when calculating the final z value (due to measurement inaccuracies).
In my case measurements may return data like this (visualized with some random online tool):
where only 2 or even none of the distance circles intersect:
What I am looking for is the an algorithm returning the best possible approximation of the target location based on three distance measurements with a known maximum error of 1 km or further approaches I could take.
I have also thought of iterating over points on the circles to then determining the minimum average distance to all the points on the other circles but the 3-dimensional sphere geometry of the earth is giving me a headache. Also there's probably a way better and simpler approach to this which I just can't figure out right now.
As this is more of an algorithmic problem, rather than any language-specific thing, I appreciate any help in whatever programming language, pseudo code or natural language.
If you have access to a scientific computing library which provides non-linear optimization utilities, then you could try finding the point which minimizes the following:
(||x - p_1|| - r_1)^2 + (||x - p_2|| - r_2)^2 + (||x - p_3|| - r_3)^2 + (||x - p_earth|| - r_earth)^2
where p_i is the location (in Cartesian coordinates) of the ith location you measure from, r_i is the corresponding distance reading, p_earth is the location of the Earth, r_earth is the radius of the earth, and ||a|| denotes the norm/length of the vector a.
Each term in the expression is trying to minimize the residual radius error.
This can of course be modified to suit your needs - e.g. if constrained optimization is available, you could encode the requirement that the point be on the surface on the earth as a constraint rather than a term to optimize for. If spherical earth model isn't accurate enough, you could define an error from the Earth's surface, or just project your result onto the Earth if that is accurate enough.
this is my first question on the forum and my algebra is rusty so please be indulgent ^^'
So my problem is that i want to predict collision between two uniform circular motion objects for which i know velocity (angular speed in radian), distance from the origin (radius), cartesian coordinate of the center of the circle.
I can get cartesian position for each object given for t time (timestamp) using :
Oa.x = ra X cos(wa X t)
Oa.y = ra X sin(wa X t)
Oa.x: Object A x coordinates
ra: radius of a Circle A
wa: velocity of object A (angular speed in radian)
t: time (timestamp)
Same goes for object b (Ob)
I want to find t such that ||Ca - Cb|| = (rOa + rOb)
rOa: radius of object a
Squaring both side and expanding give me this :
||Ca-Cb||^2 = (rOa+rOb)^2
(ra * cos (wa * t) - rb / cos (wb * t))^2 + (ra * sin (wa * t) - rb / sin (wb * t))^2 = (ra+rb)^2
From that i should get a quadratic polynomial that i can solve for t, but how can i find a condition that tell me if such a t exist ? And possibly, how to solve it for t ?
Your motion equations are missing some stuff I expect this instead:
a0(t) = omg0*t + ang0
x0(t) = cx0 + R0 * cos(a0(t))
y0(t) = cy0 + R0 * sin(a0(t))
a1(t) = omg1*t + ang1
x1(t) = cx1 + R1 * cos(a1(t))
y1(t) = cy1 + R1 * sin(a1(t))
where t is time in [sec], cx?,cy? is the center of rotation ang? is starting angle (t=0) in [rad] and omg? is angular speed in [rad/sec]. If the objects have radius r? then collision occurs when the distance is <= r0+r1
so You want to find smallest time where:
(x1-x0)^2 + (y1-y0)^2 <= (r0+r1)^2
This will most likely lead to transcendent equation so you need numeric approach to solve this. For stuff like this I usually use Approximation search so to solve this do:
loop t from 0 to some reasonable time limit
The collision will happen with constant frequency and the time between collisions will be divisible by periods of both motions so I would test up to lcm(2*PI/omg0,2*PI/omg1) time limit where lcm is least common multiple
Do not loop t through all possible times with brute force but use heuristic (like the approx search linked above) beware initial time step must be reasonable I would try dt = min(0.2*PI/omg0,0.2*PI/omg1) so you have at least 10 points along circle
solve t so the distance between objects is minimal
This however will find the time when the objects collide fully so their centers merge. So you need to substract some constant time (or search it again) that will get you to the start of collision. This time you can use even binary search as the distance will be monotonic.
next collision will appear after lcm(2*PI/omg0,2*PI/omg1)
so if you found first collision time tc0 then
tc(i) = tc0 + i*lcm(2*PI/omg0,2*PI/omg1)
i = 0,1,2,3,...
I have two known Google Geolocation points A and B. I need to return GeoLocation point C which is on AB line and on distance x from point A:
Geolocation returnGeolocationC(Geolocation A, Geolocation B, double x) {
...
return C;
}
I know that I can use Haversine formula and I can calculate AB distance and therefore I also have AC and CB distance. Any idea or hint how to implement this?
Edit: Line is straight, no need to consider roads.
Well, this is a good problem which solution will depend on the area of interest, for instance:
Consider the situation faced by a botanist studying a stand of oak trees on a small plot of land. One component of the data analysis involves determining the location of these trees and calculating the distance betwee
n them. In this situation, straight line or Euclidean distance is the most logical choice. This only requires the use of the Pythagorean Theorem to calculate the shortest distance between two points:
straight_line_distance = sqrt ( ( x2 - x1 )**2 + ( y2 - y1 )**2 );
The variables x and y refer to co-ordinates in a two-dimensional plane and can reflect any unit of measurement, such as feet or miles.
Consider a different situation, an urban area, where the objective is to calculate the distance between customers’ homes and various retail outlets. In this situation, distance takes on a more specific meaning, usually road distance, making straight line distance less suitable. Since streets in many cities are based on a grid system, the typical trip may be approximated by what is known as the Manhattan, city block or taxi cab distance (Fothering-
ham, 2002):
block_distance = ( abs( x2 - x1 ) + abs( y2 - y1 ) ) ;
Instead of the hypotenuse of the right-angled triangle that was calculated for the straight line distance, the above formula simply adds the two sides that form the right angle. The straight line and city block formulae are closely related, and can be generalized by what are referred to as the Minkowski metrics, which in this case are restricted to two dimensions:
minkowski_metric = ( abs(x2 - x1)**k + abs(y2 - y1)**k )**(1/k);
The advantage of this formula is that you only need to vary the exponent to get a range of distance measures. When k = 1, it is equivalent to the city block distance; when k=2, it is the Euclidean distance. Less commonly,
other values of k may be used if desired, usually between 1 and 2. In some situations, it may have been determined that actual distances were greater than the straight line, but less than the city block, in which case a value such as "1.4" may be more appropriate. One of the interesting features of the Minkowski metric is that for values considerably larger than 2 (approaching infinity), the distance is the larger of two sides used in the city block calculation, although this is typically not applicable in a geographic context.
So pseudocode would be something like the following:
distance2d (x1, y1, x2, y2, k)
(max( abs(x2 - x1), abs(y2 - y1) ) * (k > 2))
+
((abs(x2 - x1)**k + abs(y2 - y1)** k )**(1/ k)) * (1 <=k<=2)
end
If 1 <= k <=2, the basic Minkowski metric is applied, since (1 <= k <=2) resolves to 1 and (k > 2) resolves to 0. If k > 2, an alternate formula is applied, since computations become increasingly intensive for large values of k. This second formula is not really necessary, but is useful in demonstrating how modifications can be easily incorporated in distance measures.
The previous distance measures are based on the concept of distance in two dimensions. For small areas like cities or counties, this is a reasonable implification. For longer distances such as those that span larger countries
or continents, measures based on two dimensions are no longer appropriate, since they fail to account for the curvature of the earth. Consequently, global distance measures need to use the graticule, the co-ordinate system
comprised of latitude and longitude along with special formulae to calculate the distances. Lines of latitude run in an east to west direction either above or below the equator. Lines of longitude run north and south through the poles, often with the Prime Meridian (running through Greenwich, England) measured at 0°. Further details of latitude and longitude are available (Slocum et al., 2005). One issue with using latitude and longitude is that the co-ordinates may require some transformation and preparation before they are suitable to use in distance calculations. Coordinates are often expressed in the sexagesimal system (similar to time) of degrees, minutes, and seconds, in which each degree consists of 60 minutes and each
minute is 60 seconds. Furthermore, it is also necessary to provide and indication of the position relative to the equator (North or South) and the Prime Meridian (East or West). The full co-ordinates may take on a variety of formats; below is a typical example that corresponds approximately to the city of Philadelphia:
39° 55' 48" N 75° 12' 12" W
As you mentioned Harvesine, and also I am extending a lot, we can compare results using law of cosines and Harvesine, so pseudocode:
begin
ct = constant('pi')/180 ;
radius = 3959 ; /* 6371 km */
#Both latitude and longitude are in decimal degrees ;
lat1 = 36.12;
long1 = -86.67;
lat2 = 33.94;
long2 = -118.40 ;
#Law of Cosines ;
a = sin(lat1*ct) * sin(lat2*ct) ;
b = cos(lat1*ct) * cos(lat2*ct) * cos((long2-long1) *ct);
c = arcos(a + b) ;
d = radius * c ;
put 'Distance using Law of Cosines ' d
# Haversine ** ;
a2 = sin( ((lat2 - lat1)*ct)/2)**2 +
cos(lat1*ct) * cos(lat2*ct) * sin(((long2 - long1)*ct)/2)**2
c2 = 2 * arsin(min(1,sqrt(a2))) ;
d2 = radius * c2 ;
put 'Distance using Haversine formula =' d2
end
In addition to the constant that will be used to convert degrees to radians, the radius of the earth is required, which on average is equal to 6371 kilometres or 3959 miles. The Law of Cosines uses spherical geometry to
calculate the great circle distance for two points on the globe. The formula is analogous to the Law of Cosines for plane geometry, in which three connected great arcs correspond to the three sides of the triangle. The Haversine formula is mathematically equivalent to the Law of Cosines, but is often preferred since it is less sensitive to round-off error that can occur when measuring distances between points that are located very close tog
ether (Sinnott, 1984). With the Haversine, the error can occur for points that are on opposite sides of the earth, but this is usually less of a problem.
You can find a really easy formula at this link.
Since you have the distance from one of the points and not the fraction of the distance on the segment you can slightly modify the formula:
A=sin(d-x)/sin(d)
B=sin(x)/sin(d)
x = A*cos(lat1)*cos(lon1) + B*cos(lat2)*cos(lon2)
y = A*cos(lat1)*sin(lon1) + B*cos(lat2)*sin(lon2)
z = A*sin(lat1) + B*sin(lat2)
lat=atan2(z,sqrt(x^2+y^2))
lon=atan2(y,x)
where x is the required distance and d is the distance between A and B (that you can evaluate with Haversine), both divided by the Earth radius.
You can also use another formula for sin(d):
nx = cos(lat1)*sin(lon1)*sin(lat2) - sin(lat1)* cos(lat2)*sin(lon2)
ny = -cos(lat1)*cos(lon1)*sin(lat2) + sin(lat1)* cos(lat2)*cos(lon2)
nz = cos(lat1)*cos(lon1)*cos(lat2)*sin(lon2) - cos(lat1)*sin(lon1)*cos(lat2)*cos(lon2)
sind = sqrt(nx^2+ny^2+nz^2)
It's more complex than the Haversine formula, but you can memoize some of the factors in the two steps.
As the OP posted a non working Java implementation, this is my corrections to make it work.
private static GpsLocation CalcGeolocationWithDistance(GpsLocation pointA, GpsLocation pointB, double distanceFromA)
{ //distanceFromA = 2.0 km, PointA and PointB are in Europe on 4.0km distance.
double earthRadius = 6371000.0;
double distanceAB = CalcDistance(pointA.Latitude, pointA.Longitude, pointB.Latitude, pointB.Longitude);
//distance AB is calculated right according to Google Maps (4.0 km)
double a = Math.Sin((distanceAB - distanceFromA) / earthRadius) / Math.Sin(distanceAB / earthRadius);
double b = Math.Sin(distanceFromA / earthRadius) / Math.Sin(distanceAB / earthRadius);
double x = a * Math.Cos(pointA.Latitude * Math.PI / 180) * Math.Cos(pointA.Longitude * Math.PI / 180) + b * Math.Cos(pointB.Latitude * Math.PI / 180) * Math.Cos(pointB.Longitude * Math.PI / 180);
double y = a * Math.Cos(pointA.Latitude * Math.PI / 180) * Math.Sin(pointA.Longitude * Math.PI / 180) + b * Math.Cos(pointB.Latitude * Math.PI / 180) * Math.Sin(pointB.Longitude * Math.PI / 180);
double z = a * Math.Sin(pointA.Latitude * Math.PI / 180) + b * Math.Sin(pointB.Latitude * Math.PI / 180);
double lat = Math.Atan2(z, Math.Sqrt(x * x + y * y)) * 180 / Math.PI;
double lon = Math.Atan2(y, x) * 180 / Math.PI;
//lat and lon are mo more placed somewhere in Africa ;)
return new GpsLocation(lat, lon);
}
I am currently doing a small turn based cannon game with XNA 4.0. The game is very simple: the player chooses the speed and angle at which he desires to shoot his rocket in order to hit another player. There is also a randomly generated wind vector that affects the X trajectory of the rocket. I would like to add an AI so that the player could play against the computer in a single player mode.
The way I would like to implement the AI is very simple: find the velocity and angle that would make the rocket hit the player directly, and add a random modifier to those fields so that the AI doesn't hit another player each time.
This is the code I use in order to update the position and speed of the rocket:
Vector2 gravity = new Vector2(0, (float)400); // 400 is the sweet spot value that i have found works best for the gravity
Vector2 totalAcceleration = gravity + _terrain.WindDirection;
float deltaT = (float)gameTime.ElapsedGameTime.TotalSeconds; // Elapsed time since last update() call
foreach (Rocket rocket in _instantiatedRocketList)
{
rocket.RocketSpeed += Vector2.Multiply(gravity, deltaT); // Only changes the Y component
rocket.RocketSpeed += Vector2.Multiply(_terrain.WindDirection, deltaT); // Only changes the X component
rocket.RocketPosition += Vector2.Multiply(rocket.RocketSpeed, deltaT) + Vector2.Multiply(totalAcceleration, (float)0.5) * deltaT * deltaT;
// We update the angle of the rocket accordingly
rocket.RocketAngle = (float)Math.Atan2(rocket.RocketSpeed.X, -rocket.RocketSpeed.Y);
rocket.CreateSmokeParticles(3);
}
I know that the basic equations to find the final X and Y coordinates are:
X = V0 * cos(theta) * totalFlightTime
Y = V0 * sin(theta) * totalFlightTime - 0.5 * g * totalFlightTime^2
where X and Y are the coordinates of the player I want to hit, V0 the initial speed, theta the angle at witch the rocket is shot, totalFlightTime is, like the name says, the total flight time of the rocket until it reaches (X, Y) and g is the gravity (400 in my game).
Questions:
What I am having problems with, is knowing where to add the wind in those formulas (is it just adding "+ windDirection * totalFlightTime" in the X = equation?), and also what to do with those equations in order to do what I want to do (finding the initial speed and theta angle) since there are 3 variables (V0, theta and totalFlightTime) and only 2 equations?
Thanks for your time.
You can do this as follows:
Assuming there is no specific limit to V0 (i.e. the robot can fire the rocket at any desired speed) and using the substitutions
T=totalFlightTime
Vx=V0cos(theta)
Vy=V0sin(theta)
Choose an arbitrary value for Vx. Now your first equation simplifies to
X=VxT so T=X/Vx
to solve for T. Now substitute the value of T into the second equation and solve for Vy
Y=VyT + gT^2/2 so Vy = (Y - gT^2/2)/T
Finally you can now solve for V0 and theta
V0 = Sqrt(Vx^2 + Vy^2) and Theta = aTan(Vy/Vx)
Note that your initial choice of Vx will determine the trajectory the missile will take - if Vx is large then T will be small and the trajectory will be almost a straight line (like a bullet fired at a nearby target) - if Vx is small then T will be large and the trajectory will be an arc (like a mortar round's path). You dis start with three variables (V0, totalFlightTime, and theta) but they are dependent variables so choosing any one (or in this case Vx) plus the two equations solves for the other two. You could also pre-determine flight time and solve for Vx, Vy, theta and V0, or predetermine theta (although this would be tricky as some theta wouldn't provide a real solution.
I'm trying to figure out an algorithm for finding a random point a set distance away from a base point. So for example:
This could just be basic maths and my brain not working yet (forgive me, haven't had my coffee yet :) ), but I've been trying to work this out on paper and I'm not getting anywhere.
coordinate of point on circle with radius R and center (xc, yc):
x = xc + R*cos(a);
y = yc + R*sin(a);
changing value of angle a from 0 to 2*PI you can find any point on circumference.
Use the angle from the verticle as your random input.
Pseudocode:
angle = rand(0,1)
x = cos(angle * 2 * pi) * Radius + x_centre
y = sin(angle * 2 * pi) * Radius + y_centre
Basic Pythagoras.
Pick random number between 0 and 50 and solve h^2 = a^2 + b^2
Add a few random descisions on direction.