Finding rotation axis and angle in matrx - c#

I am trying to extract the axis of rotation and the angle of rotation from a 3x3 rotation
matrix. I am currently using this method but am unsure if I am on track. I am running into issues elsewhere in my program and am thinking this may be a cause but have been unable to verify this myself.
I am working with the XNA framework, if that helps. I looked into using Matrix.Decompose() which returns the scale (vec3) translation(vec3) and rotation(quaternion) but I am not sure that I can extract the data data I need from these values.
public static void MatrixRotationAxisAngle(Matrix m, ref float theta, ref Vector3 rot_axis) {
float trace = m.M11 + m.M22 + m.M33;
float cos_theta = 0.5f * (trace - 1.0f);
if (cos_theta > 0.999999875f) {
//allow some epsilon value
theta = 0.0f;
rot_axis = new Vector3(1.0f, 0.0f, 0.0f);
} else if (cos_theta > -0.999999875f) {
theta = (float)Math.Acos(cos_theta);
rot_axis.X = (m.Up.Z - m.Forward.Y);
rot_axis.Y = (m.Forward.X - m.Right.Z);
rot_axis.Z = (m.Right.Y - m.Up.X);
rot_axis.Normalize();
} else { //angle within PI limits
theta = (float)Math.PI;
//find index of largest diag term in matrix
int index = 0;
if (m.M11 > m.M22 && m.M11 > m.M33) {
index = 0;
}
if (m.M22 > m.M11 && m.M22 > m.M33) {
index = 1;
}
if (m.M33 > m.M11 && m.M33 > m.M22) {
index = 2;
}
switch (index) {
case 0:
float ix = 1.0f / rot_axis.X;
rot_axis.X = (float)Math.Sqrt(m.M11 + 1.0f);
rot_axis.Y = m.M12 * ix;
rot_axis.X = m.M13 * ix;
break;
case 1:
float iy = 1.0f / rot_axis.Y;
rot_axis.Y = (float)Math.Sqrt(m.M22 + 1.0f);
rot_axis.X = m.M21 * iy;
rot_axis.Z = m.M23 * iy;
break;
case 2:
float iz = 1.0f / rot_axis.Y;
rot_axis.Z = (float)Math.Sqrt(m.M33 + 1.0f);
rot_axis.X = m.M31 * iz;
rot_axis.Y = m.M32 * iz;
break;
}
rot_axis.Normalize();
}
}

I suggest to take a look into the pseudocode from this mathematical explanation (pdf), Chapter 2.1 (depending on your matrix). I have successfully used the code in one of my projects, and it returned valid results in my case.

See the matrix in http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations .
As you can see finding theta is pretty easy. Use that in other equations to find the other angles.

Related

How to find some additional points on a line between two points in 3D?

Is there a function in C# which can give me all the points on a straight line between two points in 3D?
To calculate the distance between those two points, I use this:
public class Position {
public float x;
public float y;
public float z;
}
public void CalculateDistance(Position position1, Position position2, int mapId){
float deltaX = position1.x - position2.x;
float deltaY = position1.y - position2.y;
float deltaZ = position1.z - position2.z;
float distance = (float)Math.Sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ);
Console.WriteLine("Distance is: " + distance);
}
Example coordinates:
Position pos1 = new Position();
pos1.x = 141.6586f;
pos1.y = 0.6852107f;
pos1.z = 153.2231f;
Position pos2 = new Position();
pos2.x = 142.336f;
pos2.y = 0.8685942f;
pos2.z = 130.8394f;
Let's say, the distance in line between those two 3d coordinates can be passed for 5 seconds. How can I print the current coordinate for every 1 second?
what you want to do is well described in this answer
And here is example of code how you can print your values:
var mx = pos2.x - pos1.x;
var my = pos2.y - pos1.y;
var mz = pos2.z - pos1.z;
for(var t=0; t < 10; t++) {
var x = pos1.x + mx * t;
var y = pos1.y + my * t;
var z = pos1.z + mz * t;
//TODO: use you 3D point
}
Hope this helps!

Spline path - prevent overshoot

I have a NavAgent that follows the mouse cursor. A line renderer draws the nav path from a fixed starting point to the cursor with some smoothing achieved by what - with my limited math brain - I understand to be a cubic Hermit spline, adapted from this great Wikibook page:
IEnumerator IDrawPath(Vector3 endPos)
{
while (drawLine)
{
ray = Camera.main.ScreenPointToRay(Input.mousePosition);
if (Physics.Raycast(ray, out hit, 1500))
{
transform.position = endPos;
nav.SetDestination(hit.point);
line.positionCount = numberOfPoints * (nav.path.corners.Length - 1);
for (int j = 0; j < nav.path.corners.Length - 1; j++)
{
p0 = nav.path.corners[j];
p1 = nav.path.corners[j + 1];
if (j > 0)
{
m0 = 0.5f * (nav.path.corners[j + 1] - nav.path.corners[j - 1]);
}
else
{
m0 = nav.path.corners[j + 1] - nav.path.corners[j];
}
if (j < nav.path.corners.Length - 2)
{
m1 = 0.5f * (nav.path.corners[j + 2] - nav.path.corners[j]);
}
else
{
m1 = nav.path.corners[j + 1] - nav.path.corners[j];
}
pointStep = 1.0f / numberOfPoints;
if (j == nav.path.corners.Length - 2)
{
pointStep = 1.0f / (numberOfPoints - 1.0f);
}
for (int i = 0; i < numberOfPoints; i++)
{
t = i * pointStep;
position = (2.0f * t * t * t - 3.0f * t * t + 1.0f) * p0
+ (t * t * t - 2.0f * t * t + t) * m0
+ (-2.0f * t * t * t + 3.0f * t * t) * p1
+ (t * t * t - t * t) * m1;
line.SetPosition(i + j * numberOfPoints,
position);
}
}
}
yield return new WaitForFixedUpdate();
}
}
Sometimes the line 'overshoots' an optimal curve position and doubles back on itself:
I'd like the line to proceed smoothly round those corners instead.
From reading up on splines from various sources I gather this behaviour is correlated to the values of the tangents being calculated (the variables M0 and M1 in this case). I have adjusted these without success; larger values cause the line to stray away from the path points (and increase the size of the overshoot), smaller values cause a 'knot' rather than a loop as the resultant positions are very close together.
I tried filtering out some of the extreme points, hoping this might lead to a smoother line by adding some logic to say if the angle of the next point is above a certain threshold don't add it to the line renderer positions but this yields some odd behaviour. I've tried filtering out points that are very close together, again leading to some odd results.
This is probably simple stuff for a math wizz but I'm all out of ideas. How can I avoid the line doubling back on itself. I've readthrough Monotone cubic interpolation which seems promising but frankly it's way over my head. Can the calculation above be adjusted to do this, or do I need to look for another way?

C# / OpenTK, why does my sphere not look smooth?

This should hopefully be a simple question. So I finally figured out how to render stuff in 3D in OpenTK. Great! Only problem is, it doesn't quite look how I expect. I'm drawing a sphere using the Polar method, and drawing using PrimitiveType.Polygon.
Here's the algorithm for calculating the coordinates. What I'm doing is stepping through each phi then theta in the sphere, incrementally adding more adjacent quads to my final point list:
Point 1: Theta1, Phi1
Point 2: Theta1, Phi2
Point 3: Theta2, Phi2
Point 4: Theta2: Phi1
protected static RegularPolygon3D _create_unit(int n)
{
List<Vector3> pts = new List<Vector3>();
float theta = 0.0f;
float theta2 = 0.0f;
float phi = 0.0f;
float phi2 = 0.0f;
float segments = n;
float cosT = 0.0f;
float cosT2 = 0.0f;
float cosP = 0.0f;
float cosP2 = 0.0f;
float sinT = 0.0f;
float sinT2 = 0.0f;
float sinP = 0.0f;
float sinP2 = 0.0f;
List<Vector3> current = new List<Vector3>(4);
for (float lat = 0; lat < segments; lat++)
{
phi = (float)Math.PI * (lat / segments);
phi2 = (float)Math.PI * ((lat + 1.0f) / segments);
cosP = (float)Math.Cos(phi);
cosP2 = (float)Math.Cos(phi2);
sinP = (float)Math.Sin(phi);
sinP2 = (float)Math.Sin(phi2);
for (float lon = 0; lon < segments; lon++)
{
current = new List<Vector3>(4);
theta = TWO_PI * (lon / segments);
theta2 = TWO_PI * ((lon + 1.0f) / segments);
cosT = (float)Math.Cos(theta);
cosT2 = (float)Math.Cos(theta2);
sinT = (float)Math.Sin(theta);
sinT2 = (float)Math.Sin(theta2);
current.Add(new Vector3(
cosT * sinP,
sinT * sinP,
cosP
));
current.Add(new Vector3(
cosT * sinP2,
sinT * sinP2,
cosP2
));
current.Add(new Vector3(
cosT2 * sinP2,
sinT2 * sinP2,
cosP2
));
current.Add(new Vector3(
cosT2 * sinP,
sinT2 * sinP,
cosP
));
pts.AddRange(current);
}
}
var rtn = new RegularPolygon3D(pts);
rtn.Translation = Vector3.ZERO;
rtn.Scale = Vector3.ONE;
return rtn;
}
And so my Sphere class looks like this:
public class Sphere : RegularPolygon3D
{
public static Sphere Create(Vector3 center, float radius)
{
var rp = RegularPolygon3D.Create(30, center, radius);
return new Sphere(rp);
}
private Sphere(RegularPolygon3D polygon) : base(polygon)
{
}
}
I should also mention, that the color of this sphere is not constant. I 2 dimensions, I have this code that works great for gradients. In 3D...not so much. That's why my sphere has multiple colors. The way the 2d gradient code works, is there is a list of colors coming from a class I created called GeometryColor. When the polygon is rendered, every vertex gets colored based off the list of colors within GeometryColor. So if there are 3 colors the user wished to gradient between, and there were 6 vertices (hexagon), then the code would assign the first 2 vertices color 1, the 2nd two color 2, then the last 2 color 3. The following code shows how the color for the vertex is calculated.
public ColorLibrary.sRGB GetVertexFillColor(int index)
{
var pct = ((float)index + 1.0f) / (float)Vertices.Count;
var colorIdx = (int)Math.Round((FillColor.Colors.Count - 1.0f) * pct);
return FillColor.Colors[colorIdx];
}
Anyway, here's the output I'm getting...hope somebody can see my error...
Thanks.
Edit: If I only use ONE Vertex color (i,e instead of my array of 4 diff colors), then I get a completely smooth sphere...although without lighting and stuff its hard to tell its anything but a circle lol)
Edit....so somehow my sphere is slightly see through...even though all my alphas are set to 1.0f and I'm doing depth testing..
GL.DepthMask(true);
GL.Enable(EnableCap.DepthTest);
GL.ClearDepth(1.0f);
GL.DepthFunc(DepthFunction.Lequal);
Final edit: OK, it has SOMETHING to do with my vertices I'm guessing, because when I use PrimitiveType.Quads it works perfectly....

How do I properly setup a texture position using XNA/Monogame VertexPositionTexture on a circle

I am using the following to create a circle using VertexPositionTexture:
public static ObjectData Circle(Vector2 origin, float radius, int slices)
{
/// See below
}
The texture that is applied to it doesn't look right, it spirals out from the center. I have tried some other things but nothing does it how I want. I would like for it to kind-of just fan around the circle, or start in the top-left end finish in the bottom-right. Basically wanting it to be easier to create textures for it.
I know that are MUCH easier ways to do this without using meshes, but that is not what I am trying to accomplish right now.
This is the code that ended up working thanks to Pinckerman:
public static ObjectData Circle(Vector2 origin, float radius, int slices)
{
VertexPositionTexture[] vertices = new VertexPositionTexture[slices + 2];
int[] indices = new int[slices * 3];
float x = origin.X;
float y = origin.Y;
float deltaRad = MathHelper.ToRadians(360) / slices;
float delta = 0;
float thetaInc = (((float)Math.PI * 2) / vertices.Length);
vertices[0] = new VertexPositionTexture(new Vector3(x, y, 0), new Vector2(.5f, .5f));
float sliceSize = 1f / slices;
for (int i = 1; i < slices + 2; i++)
{
float newX = (float)Math.Cos(delta) * radius + x;
float newY = (float)Math.Sin(delta) * radius + y;
float textX = 0.5f + ((radius * (float)Math.Cos(delta)) / (radius * 2));
float textY = 0.5f + ((radius * (float)Math.Sin(delta)) /(radius * 2));
vertices[i] = new VertexPositionTexture(new Vector3(newX, newY, 0), new Vector2(textX, textY));
delta += deltaRad;
}
indices[0] = 0;
indices[1] = 1;
for (int i = 0; i < slices; i++)
{
indices[3 * i] = 0;
indices[(3 * i) + 1] = i + 1;
indices[(3 * i) + 2] = i + 2;
}
ObjectData thisData = new ObjectData()
{
Vertices = vertices,
Indices = indices
};
return thisData;
}
public static ObjectData Ellipse()
{
ObjectData thisData = new ObjectData()
{
};
return thisData;
}
ObjectData is just a structure that contains an array of vertices & an array of indices.
Hope this helps others that may be trying to accomplish something similar.
It looks like a spiral because you've set the upper-left point for the texture Vector2(0,0) in the center of your "circle" and it's wrong. You need to set it on the top-left vertex of the top-left slice of you circle, because 0,0 of your UV map is the upper left corner of your texture.
I think you need to set (0.5, 0) for the upper vertex, (1, 0.5) for the right, (0.5, 1) for the lower and (0, 0.5) for the left, or something like this, and for the others use some trigonometry.
The center of your circle has to be Vector2(0.5, 0.5).
Regarding the trigonometry, I think you should do something like this.
The center of your circle has UV value of Vector2(0.5, 0.5), and for the others (supposing the second point of the sequence is just right to the center, having UV value of Vector2(1, 0.5)) try something like this:
vertices[i] = new VertexPositionTexture(new Vector3(newX, newY, 0), new Vector2(0.5f + radius * (float)Math.Cos(delta), 0.5f - radius * (float)Math.Sin(delta)));
I've just edited your third line in the for-loop. This should give you the UV coordinates you need for each point. I hope so.

Creating a spiral galaxy need math guidance

I am creating a spinning galaxy made of blocks for the stars/systems.
I have been fiddling with this for a few days and have this so far:
public int numberArms = 6;
public int numberStars = 1000;
public float galaxyRadius = 500f;
public int spread = 100;
float fHatRandom (float fRange)
{
float fArea = 4 * Mathf.Atan (6.0f);
float fP = fArea * Random.value;
return Mathf.Tan (fP / 4) * fRange / 6.0f;
}
float fLineRandom (float fRange)
{
float fArea = fRange * fRange / 2;
float fP = fArea * Random.value;
return fRange - Mathf.Sqrt (fRange * fRange - 2 * fP);
}
// Use this for initialization
void Start ()
{
Random.seed = 100;
int starsPerArm = numberStars / numberArms;
float fAngularSpread = spread / numberArms;
float fArmAngle = (360 / numberArms);
for (int arm = 0; arm < numberArms; arm++)
{
for (int i = 0; i < starsPerArm; i++)
{
float fR = fHatRandom (galaxyRadius);
float fQ = fLineRandom (fAngularSpread);
float fK = 1;
float fA = numberArms * (fArmAngle);
float fX = fR * Mathf.Cos (Mathf.Deg2Rad * (fA + fR * fK + fQ));
float fY = fR * Mathf.Sin (Mathf.Deg2Rad * (fA + fR * fK + fQ));
Vector3 starPos = new Vector3 (fX, fY, arm*4);
Collider[] colliders = Physics.OverlapSphere (starPos, 1);
if (colliders.Length == 0)
{
GameObject star = GameObject.CreatePrimitive (PrimitiveType.Cube);
star.transform.position = starPos;
star.transform.parent = transform;
} else
{
i--;//because they overlapped, we try again.
}
}
}
}
}
As it works right now, it creates the spiral arm of the galaxy just fine. But as you can see, I just set the position of the arm to be stacked on the other arms because I cannot for the life of me figure out how to get them to rotate around the center, for that matter my center seems to be off.
I admittedly have the math skills of a gnat and have been fumbling my way through this, can someone help correct the math and get the arms/center where they belong?
Shouldn't that:
float fA = numberArms * (fArmAngle);
be
float fA = arm * (fArmAngle);
Just saying...

Categories

Resources