We Keep Coding

How can I calculate an average terrain slope based on four points?

I'm programming a simple physics system for vehicles in Unity and I'm trying to figure out how to calculate the average slope based on four points in order to apply the gravitational acceleration. I can get the terrain height at the vehicle wheels positions like this:
Terrain.activeTerrain.SampleHeight(wheel.position);
How can I calculate the average terrain slope based on those four points? I thought about calculating direction vectors between different points, but I'm not sure how to combine them into a single slope vector.

(Assuming that all 4 wheels are always coplanar) I think you could take any 3 wheels, take the height sample
var wheels = new Vector3[3]{ wheel1.transform.position, wheel2.transform.position, wheel3.transform.position };
for(var i = 0; i < wheels.Length; i++)
{
var w = wheels[i];
w.y = Terrain.activeTerrain.SampleHeight(w)
wheels[i]= w;
}
and from that resulting 3 points generate a plane
var plane = new Plane(wheels[0], wheels[1], wheels[2]);
Now you can take any of these points and take a vector in straight up direction.
var upPoint = wheels[0] + Vector3.up;
Then you can map that vector onto the plane using closest point on plane.
var planePoint = plane.ClosestPointOnPlane(upPoint);
As a last step normalize the vector between that mapped point and your start wheel point and you should have the direction of the slope + the Y component tells you how steep it is
var planeVector = wheels[0] - planePoint;
var direction = planeVector.normalized;
Debug.Log($"Slope direction is {direction} with a steepness factor of {direction.y} or also {-planeVector.magnitude}");
where both planeVector.magnitude and direction.y should be moving between 0 (completely horizontal surface) and -1 (completely vertical surface)
Something like that I guess ^^

Wouldn't that be the average slope between the middle point of the back wheels and the middle point of the front wheels? You get the tangent of the angle by dividing the difference between those heights and their distance (as projected). Guess that's too simple?
Assuming 4 points (x1,y1), (x2,y2), (x3,y3) and (x4,y4). The middlepoints would be xB = (x1+x2)/2, yB=(y1+y2)/2 and xF=(x3+x4)/2 and yF=(y3+y4)/2. Then Tan A = (yF-yB)/(xF-xB)

How to find the area that two triangles are in contact

Given the positions of the vertices, and the surface normals. How can I calculate the area(which may be 0) that the 2 triangles are in contact? These triangles are also in 3D space so if they aren't lined up properly, just jammed into each other, the contact area should be 0.
(In C#)

This is not a trivial problem, so let's break it down into steps.
Check if the two triangles are coplanar, otherwise the area is 0.
Project the triangles onto a 2D surface
Calculate the intersection polygon
Calculate the area
1. Checking for coplanarity
For the two triangles to be coplanar, all vertices of one triangle must lie in the plane determined by the other one.
Using the algorithm described here we can check for every vertex whether that is the case, but due to the fact that floating point numbers are not perfectly precise, you will need to define some error theshold to determine what still counts as coplanar.
Assuming va and vb are the vectors of the triangles A and B respectively, the code could look something like this.
(Note: I have never worked with Unity and am writing all of the code from memory, so please excuse if it isn't 100% correct).
public static bool AreTrianglesCoplanar(Vector3[] va, Vector3[] vb) {
// make sure these are actually triangles
Debug.Assert(va.Length == 3);
Debug.Assert(vb.Length == 3);
// calculate the (scaled) normal of triangle A
var normal = Vector3.Cross(va[1] - va[0], va[2] - va[0]);
// iterate all vertices of triangle B
for(var vertex in vb) {
// calculate the dot product between the normal and the vector va[0] --> vertex
// the dot product will be 0 (or very small) if the angle between these vectors
// is a right angle (90°)
float dot = Vector3.Dot(normal, vertex - va[0]).
// the error threshold
const float epsilon = 0.001f;
// if the dot product is above the threshold, the vertex lies outside the plane
// in that case the two triangles are not coplanar
if(Math.Abs(dot) > epsilon)
return false;
}
return true;
}
2. Projecting the triangles into 2D space
We now know that all six vertices are in the same 2D plane embedded into 3D space, but all of our vertex coordinates are still three-dimensional. So the next step would be to project our points into a 2D coordinate system, such that their relative position is preserved.
This answer explains the math pretty well.
First, we need to find a set of three vectors forming an orthonormal basis (they must be orthoginal to each other and of length 1).
One of them is just the plane's normal vector, so we need two more vectors that are orthogonal to the first, and also orthogonal to each other.
By definition, all vectors in the plane defined by our triangles are orthogonal to the normal vector, so we can just pick one (for example the vector from va[0] to va[1]) and normalize it.
The third vector has to be orthogonal to both of the others, we can find such a vector by taking the cross product of the previous two.
We also need to choose a point in the plane as our origin point, for example va[0].
With all of these parameters, and using the formula from the linked amswer, we can determine our new projected (x, y) coordinates (t_1 and t_2 from the other answer). Note that -- because all of our points lie in the plane defining that normal vector -- the third coordinate (called s in the other answer) will always be (close to) zero.
public static void ProjectTo2DPlane(
Vector3[] va, Vector3[] vb
out Vector2[] vaProjected, out Vector2[] vbProjecte
) {
// calculate the three coordinate system axes
var normal = Vector3.Cross(va[1] - va[0], va[2] - va[0]).normalized;
var e1 = Vector3.Normalize(va[1] - va[0]);
var e2 = Vector3.Cross(normal, e1);
// select an origin point
var origin = va[0];
// projection function we will apply to every vertex
Vector2 ProjectVertex(Vector3 vertex) {
float s = Dot(normal, vertex - origin);
float t1 = Dot(e1, vertex - origin);
float t2 = Dot(e2, vertex - origin);
// sanity check: this should be close to zero
// (otherwise the point is outside the plane)
Debug.Assert(Math.Abs(s) < 0.001);
return new Vector2(t1, t2);
}
// project the vertices using Linq
vaProjected = va.Select(ProjectVertex).ToArray();
vbProjected = vb.Select(ProjectVertex).ToArray();
}
Sanity check:
The vertex va[0] should be projected to (0, 0).
The vertex va[1] should be projected to (*, 0), so somewhere on the new x axis.
3. / 4. Calculating the intersection area in 2D
This answer this answer mentions the algorithms necessary for the last step.
The Sutherland-Hodgman algorithm successively clips one triangles with each side of the other. The result of this will be either a triangle, a quadrilateral or an empty polygon.
Finally, the shoelace formula can be used to calculate the area of the resulting clipping polygon.
Bringing it all together
Assuming you implemented the two functions CalculateIntersecionPolygon and CalculatePolygonArea, the final intersection area could be calculated like this:
public static float CalculateIntersectionArea(Mesh triangleA, Mesh triangleB) {
var verticesA = triangleA.GetVertices();
var verticesB = triangleB.GetVertices();
if(!AreTrianglesCoplanar(verticesA, verticesB))
return 0f; // the triangles are not coplanar
ProjectTo2DPlane(verticesA, verticesB, out Vector2[] projectedA, out Vector2[] projectedB);
CalculateIntersecionPolygon(projectedA, projectedB, out List<Vector2> intersection);
if(intersection.Count == 0)
return 0f; // the triangles didn't overlap
return CalculatePolygonArea(intersection);
}

Function to generate flight trajectory (list of 3D points, lat, lon, alt)

I am looking to generate some 3D trajectory data for an aircraft simulation.
The idea is that the aircraft takes off at some location x and continues to ascend at some average ascent velocity a_v and angle a_theta until it reaches a maximum altitude m_a. The aircraft would then continue at its m_a until it reaches a certain distance d_d from its destination, at which point it will begin its descent at some angle d_theta with an average descent velocity of d_v. Finally, the aircraft lands at destination y.
I would like the function to return a list of 3D points.
I am looking to implement this in either Python (preferred) or C#.
For illustration purposes:
Does anyone know how I can achieve this? Is there perhaps some open source project which does this? I have been looking for a while now, but have not found anything.

I recommend you to solve the problem in 2 independent steps so that the airplane does not pass through the ground :
Calculate the path on the surface of a sphere.
Interpolate the height along this path.
For 1. you can use the spherical interpolation techniques on Quaternions.
Quaternion slerp(Quaternion v0, Quaternion v1, double t) {
// Only unit quaternions are valid rotations.
// Normalize to avoid undefined behavior.
v0.normalize();
v1.normalize();
// Compute the cosine of the angle between the two vectors.
double dot = dot_product(v0, v1);
const double DOT_THRESHOLD = 0.9995;
if (fabs(dot) > DOT_THRESHOLD) {
// If the inputs are too close for comfort, linearly interpolate
// and normalize the result.
Quaternion result = v0 + t*(v1 – v0);
result.normalize();
return result;
}
// If the dot product is negative, the quaternions
// have opposite handed-ness and slerp won't take
// the shorter path. Fix by reversing one quaternion.
if (dot < 0.0f) {
v1 = -v1;
dot = -dot;
}
Clamp(dot, -1, 1); // Robustness: Stay within domain of acos()
double theta_0 = acos(dot); // theta_0 = angle between input vectors
double theta = theta_0*t; // theta = angle between v0 and result
Quaternion v2 = v1 – v0*dot;
v2.normalize(); // { v0, v2 } is now an orthonormal basis
return v0*cos(theta) + v2*sin(theta);
}

You didn't write any code, so I won't write any either. Python with math package is more than enough to solve this problem.
Required steps:
The plane should fly on a great circle. This means you only need one distance to describe X and Y.
You could place the origin at X and specify Y with a latitude.
Calculate the tangent of the Earth at X, and rotate by a_theta. Find the point where it reaches m_a altitude.
Calculate the tangent of the Earth at Y, and rotate by d_theta. Find the point where it reaches m_a altitude.
Draw an arc between the two previous points, with a radius of EarthRadius + m_a
Every coordinate is known in the 2D of the great circle, you just need to rotate them back to 3D coordinates.
For a list of 3D points, you don't need either a_v, d_v or d_d.

Kinect 2 Inconsistent FloorClipPlane reading

I have successfully implemented the floor clip plane to measure the distance of left foot to the floor, which is fairly accurate. The problem I have is that as I move away from the camera (i.e. left foot Z axis is increased), the foot distance to the floor changes (increases).
Note: The floor itself is not tilted nor the Kinect stand.
I tested it with Kinect 1 and had the same result. The subject's head height (Y axis) also changes value as I move away or get closer to the camera. It does not matter of the camera is tilted or line of sight. the D value in the FloorClipPlane equation shows a constant number during the test.
A = bodyFrame.FloorClipPlane.X;
B = bodyFrame.FloorClipPlane.Y;
C = bodyFrame.FloorClipPlane.Z;
D = bodyFrame.FloorClipPlane.W;
distanceLeftFoot = A * leftFootPosX + B * leftFootPosY + C * leftFootPosZ + D;
Just to let you know, I have coordinate mapping between depth and colour. Not sure if that has anything to do with the issue.

The FloorClipPlane is expressed in hessian normal form - as explained in the docs. Specifically, your A, B, and C values compromise the unit vector from camera origin (center of the Kinect) to floor plane such that it produces a perpendicular intersection with the floor plane. D is the magnitude of that vector (distance from camera origin to floor plane).
Even if you think the floor is flat and the Kinect is parallel to the ground, you have a perspective warping problem which means the body location (measured in depth space) is going to change as you come closer and further.
To fix this you need to provide as input both your 3D coordinate values and the floor plane, which will then give you back what you want, a measured distance from floor plane to joint:
// j is your joint - left foot or any other joint
float x = j.Position.X;
float y = j.Position.Y;
float z = j.Position.Z;
float distance = (Math.Abs((x * floorPlane.X) + (y * floorPlane.Y) + (z * floorPlane.Z) + floorPlane.W))/((float)Math.Sqrt((Math.Pow(floorPlane.X,2)) + (Math.Pow(floorPlane.Y, 2)) + (Math.Pow(floorPlane.Z, 2))));
I hope this helps you. Can't elaborate further what influence your mapping from depth to color might be doing here without seeing what you are specifically doing

C# code snippet calculating the surface and vertex normals

I need a C# code snippet calculating the surface and vertex normals. Kind of surface is triangulated 3D closed mesh. The required code snippet must be able to use a vertex set and triangleindices. These are ready to use at the moment. The surface of 3D mesh object is not smooth, so it needs to be smoothed.
Could you help me.

It sounds like you're trying to display your 3D mesh and apply a smooth shading appearance by interpolating surface normals, such as in Phong shading, and you need to calculate the normals first. This is different from smoothing the surface of the mesh itself, since that implies altering the positions of its vertices.
Surface normals can be calculated by getting the vector cross product of two edges of a triangle.
As far as code, I'm unaware of any C# examples, but here is one in C++ that should be easy to port. It is taken from the popular NeHe tutorials for OpenGL:
void calcNormal(float v[3][3], float out[3]) // Calculates Normal For A Quad Using 3 Points
{
float v1[3],v2[3]; // Vector 1 (x,y,z) & Vector 2 (x,y,z)
static const int x = 0; // Define X Coord
static const int y = 1; // Define Y Coord
static const int z = 2; // Define Z Coord
// Finds The Vector Between 2 Points By Subtracting
// The x,y,z Coordinates From One Point To Another.
// Calculate The Vector From Point 1 To Point 0
v1[x] = v[0][x] - v[1][x]; // Vector 1.x=Vertex[0].x-Vertex[1].x
v1[y] = v[0][y] - v[1][y]; // Vector 1.y=Vertex[0].y-Vertex[1].y
v1[z] = v[0][z] - v[1][z]; // Vector 1.z=Vertex[0].y-Vertex[1].z
// Calculate The Vector From Point 2 To Point 1
v2[x] = v[1][x] - v[2][x]; // Vector 2.x=Vertex[0].x-Vertex[1].x
v2[y] = v[1][y] - v[2][y]; // Vector 2.y=Vertex[0].y-Vertex[1].y
v2[z] = v[1][z] - v[2][z]; // Vector 2.z=Vertex[0].z-Vertex[1].z
// Compute The Cross Product To Give Us A Surface Normal
out[x] = v1[y]*v2[z] - v1[z]*v2[y]; // Cross Product For Y - Z
out[y] = v1[z]*v2[x] - v1[x]*v2[z]; // Cross Product For X - Z
out[z] = v1[x]*v2[y] - v1[y]*v2[x]; // Cross Product For X - Y
ReduceToUnit(out); // Normalize The Vectors
}
The normalization function ReduceToUnit() can be found there as well.
Note that this calculates a surface normal for a single triangle. Since you give no information about how your vertices and indices are stored, I will leave it up to you to derive the set of triangles you need to pass to this function.
EDIT: As an additional note, I think the "winding direction" of your triangles is significant. Winding in the wrong direction will cause the normal to point in the opposite direction as well.