We Keep Coding

Quaternion vector roation

My quaternion math is a bit rusty, and I'm trying to figure out which of the following implementations is more correct...
Looking here you can see Microsoft's version of transforming a vector by a quaternion: https://referencesource.microsoft.com/#System.Numerics/System/Numerics/Vector3.cs,347
Here is the code, and it has clearly been optimized:
public static Vector3 Transform(Vector3 value, Quaternion rotation)
{
float x2 = rotation.X + rotation.X;
float y2 = rotation.Y + rotation.Y;
float z2 = rotation.Z + rotation.Z;
float wx2 = rotation.W * x2;
float wy2 = rotation.W * y2;
float wz2 = rotation.W * z2;
float xx2 = rotation.X * x2;
float xy2 = rotation.X * y2;
float xz2 = rotation.X * z2;
float yy2 = rotation.Y * y2;
float yz2 = rotation.Y * z2;
float zz2 = rotation.Z * z2;
return new Vector3(
value.X * (1.0f - yy2 - zz2) + value.Y * (xy2 - wz2) + value.Z * (xz2 + wy2),
value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2) + value.Z * (yz2 - wx2),
value.X * (xz2 - wy2) + value.Y * (yz2 + wx2) + value.Z * (1.0f - xx2 - yy2));
}
However, this gives different results to my own (less optimized) implementation:
public static Vector3 Transform(this Vector3 value, Quaternion rotation)
{
var q = new Quaternion(value.X, value.Y, value.Z, 0.0f);
var res = rotation.Conjugate() * q * rotation;
return new Vector3(res.X, res.Y, res.Z);
}
public static Quaternion operator *(Quaternion value1, Quaternion value2)
{
// 9 muls, 27 adds
var tmp_00 = (value1.Z - value1.Y) * (value2.Y - value2.Z);
var tmp_01 = (value1.W + value1.X) * (value2.W + value2.X);
var tmp_02 = (value1.W - value1.X) * (value2.Y + value2.Z);
var tmp_03 = (value1.Y + value1.Z) * (value2.W - value2.X);
var tmp_04 = (value1.Z - value1.X) * (value2.X - value2.Y);
var tmp_05 = (value1.Z + value1.X) * (value2.X + value2.Y);
var tmp_06 = (value1.W + value1.Y) * (value2.W - value2.Z);
var tmp_07 = (value1.W - value1.Y) * (value2.W + value2.Z);
var tmp_08 = tmp_05 + tmp_06 + tmp_07;
var tmp_09 = (tmp_04 + tmp_08) * 0.5f;
return new Quaternion(
tmp_01 + tmp_09 - tmp_08,
tmp_02 + tmp_09 - tmp_07,
tmp_03 + tmp_09 - tmp_06,
tmp_00 + tmp_09 - tmp_05
);
}
Since these do not give the same results, one of them must be wrong, but which one is it and why?
My own implementation seems to work in the cases that I'm trying to use it in, and the MS implementation seems to be broken, but I would be surprised if it were actually incorrect, since I think it is very widely used...
Thanks! :)

There are two different conventions for rotating a vector via a quaternion, left chain and right chain. E.g.,
vnew = q * v * conjugate(q) <-- This is left chain (successive rotations stack up on the left)
vnew = conjugate(q) * v * q <-- This is right chain (successive rotations stack up on the right)
Left chain convention is typically used for active rotations, where the quaternion is being used to rotate a vector within a coordinate frame. I.e., v and vnew are two different vectors coordinatized within the same coordinate frame.
Right chain convention is typically used for passive rotations, e.g. quaternions that represent coordinate system transformations. I.e., v and vnew are actually the same vector, just coordinatized in two different coordinate frames.
The MS code you show above is a left chain convention, but your code is a right chain convention. Hence the different results.
Both conventions are valid and have their uses, but you need to be very careful when pulling code from online sources. You need to verify what the convention used by the code is in order to use it correctly. And you need to ensure the convention matches how you are using the quaternions in your particular application.

C# create earthmap dictionary of latitudes and longitudes with distance 100km

I am trying to figure math for getting all longitudes and latitudes in a specific distance between each other. I have math for getting distance between geographic coords but I would like to get an array of all geo coords from the standard plane earthmap in the specific distance. Can you help me, please?
Here is example link of earthmap picture. I have earth heightmap in resolution 43200x21600 and I would like to get heights/pixels indexed from bottom left to top right in exact distance 100km. If I can get longitudes and latitudes in distance 100km then I can read these heights.
Here is math for distance between geographic points:
public static float GetGeographicDistance(float latitudeA, float longitudeA, float latitudeB, float longitudeB, float earthRadius = 6371.007f)
{
float phi1 = latitudeA * Mathf.Deg2Rad;
float phi2 = latitudeB * Mathf.Deg2Rad;
float deltaPhi = (latitudeB - latitudeA) * Mathf.Deg2Rad;
float deltaLambda = (longitudeB - longitudeA) * Mathf.Deg2Rad;
float a = Mathf.Sin(deltaPhi / 2) * Mathf.Sin(deltaPhi / 2) +
Mathf.Cos(phi1) * Mathf.Cos(phi2) *
Mathf.Sin(deltaLambda / 2) * Mathf.Sin(deltaLambda / 2);
float c = 2.0f * Mathf.Atan2(Mathf.Sqrt(a), Mathf.Sqrt(1.0f - a));
return earthRadius * c;
}

Got it. If somebody needs it, here is the function in C#.
public static TGeographicCoordinate DestinationPointFromStartPoint(TGeographicCoordinate start, double distance = 100f, double bearing = 90f, double earthRadius = 6371.009d)
{
TGeographicCoordinate result = new TGeographicCoordinate();
double angularDistanceRdn = distance / earthRadius;
double bearingRad = Mathf.Deg2Rad * bearing;
double startLatRad = start.Latitude * Mathf.Deg2Rad;
double startLonRad = start.Longitude * Mathf.Deg2Rad;
result.Latitude = Math.Asin((Math.Sin(startLatRad) * Math.Cos(angularDistanceRdn)) +
(Math.Cos(startLatRad) * Math.Sin(angularDistanceRdn) * Math.Cos(bearingRad)));
result.Longitude = startLonRad + Math.Atan2(Math.Sin(bearingRad) * Math.Sin(angularDistanceRdn) * Math.Cos(startLatRad),
Math.Cos(angularDistanceRdn) - (Math.Sin(startLatRad)) * Math.Sin(result.Latitude));
result.Latitude *= Mathf.Rad2Deg;
result.Longitude *= Mathf.Rad2Deg;
return result;
}

C# Create 3x3 Matrix from vector

I have a vector and I want to create a Matrix from this vector. The Matrix3 should be 3times Vector3. How is it possible? Do I have to create a new public static?
The Code below shows my Vector3.
public static Vector3 VectorMultiply(Vector3 v, Matrix3 m)
{
Vector3 vr = new Vector3();
vr.x = v.x * m.X.x + v.y * m.Y.x + v.z * m.Z.x;
vr.y = v.x * m.X.y + v.y * m.Y.y + v.z * m.Z.y;
vr.z = v.x * m.X.z + v.y * m.Y.z + v.z * m.Z.z;
return (vr);

Calculating angles between two points in 3d space

I have two points (cube game object) in Unity, P1(x,y,z) and P2(x,y,z) and I have to set the MainCamera position and rotation along the vector between P2P1.
I tried different methods but was not successful.
All suggestions are welcomed.
Thanks.

We can find angle between 2 vectors according the dot production.
angle = arccos ( a * b / |a| * |b| );
where:
a * b = ax * bx + ay * by + az * bz
|a| = sqrt( ax * ax + ay * ay + az * az )
|b| = sqrt( bx * bx + by * by + bz * bz )
Or just use this method: http://docs.unity3d.com/ScriptReference/Vector3.Angle.html

Collision response and elastic impulse in XNA 4.0

I know there are physic plugins for C# or XNA, but I want to create my own, so I can learn about the topic.
My problems are the following:
I try to apply an elastic impulse to my character with the right angle and velocity. The velocity is calculated the right way, the angle is not and distorts the results!
The next problem is, that my character gets into a shaking mode, though it should stand still. I know where the problem comes from, but I don't know how to fix it (edit: do I have to consider the penetration depth for that?)
The IPhysicsObject inherits the most important informations, the Vector2[] has the collisionPoint at index 0 and the penetration depth at index 1.
I have tried to work with this but yeah.. I don't know
public void ElasticImpulse(IPhysicsObject Object, Vector2[] _colPos)
{
//this function is down below
if (checkCollidingObjects(m_cCharacter, Object))
return;
//this List is like this declined:
//public static List<IPhysicsObject[]> CollidingObjects = new List<IPhysicsObject[]>();
//this list contains every pair of objects, that collided this frame, it is cleared after all physics and game logic is done.
CollidingObjects.Add(new IPhysicsObject[] { m_cCharacter, Object });
//deltavelocity is the velocity between two frames
Vector2 v1 = Velocity - DeltaVelocity;
float lv1 = (float)Math.Sqrt(v1.X * v1.X + v1.Y * v1.Y);
float m1 = Mass;
float k1 = Damping;
Vector2 v2 = Object.Physik.Velocity - Object.Physik.DeltaVelocity;
float lv2 = (float)Math.Sqrt(v2.X * v2.X + v2.Y * v2.Y);
float m2 = Object.Mass;
float k2 = Object.Physik.Damping;
Vector2 colDir1 = _colPos[0] - m_cCharacter.Position;
Vector2 colDir2 = _colPos[0] - Object.Position;
colDir1.Normalize();
colDir2.Normalize();
Vector2 colNorm1 = new Vector2(colDir1.Y, -colDir1.X);
Vector2 colNorm2 = new Vector2(colDir2.Y, -colDir2.X);
float ldir1 = (float)Math.Sqrt(colNorm1.X * colNorm1.X + colNorm1.Y * colNorm1.Y);
float ldir2 = (float)Math.Sqrt(colNorm2.X * colNorm2.X + colNorm2.Y * colNorm2.Y);
float pi = MathHelper.Pi;
//float angle1 = pi - ((v1.X * colNorm1.X + v2.Y * colNorm1.Y) / (lv1 * ldir1)) / v1.Length();
float angle1 = pi - (float)Math.Acos(((v1.X * colNorm1.X + v2.Y * colNorm1.Y) / (lv1 * ldir1)) / v1.Length());
angle1 = (float.IsNaN(angle1)) ? 0 : angle1;
//float angle2 = pi - ((v2.X * colNorm2.X + v2.Y * colNorm2.Y) / (lv2 * ldir1)) / v2.Length();
float angle2 = pi - (float)Math.Acos(((v2.X * colNorm2.X + v2.Y * colNorm2.Y) / (lv2 * ldir1)) / v2.Length());
angle2 = (float.IsNaN(angle2)) ? 0 : angle2;
//calculating the new velocities u 1/2. Got this formula out of the wiki link i posted above (took the german wiki version)
Vector2 u1 = (m1 * v1 + m2 * v2 - (m2 * (v1 - v2) * k2)) / (m1 + m2) - v1;
Vector2 u2 = (m1 * v1 + m2 * v2 - (m1 * (v2 - v1) * k1)) / (m1 + m2) - v2;
//transform the new velocities by the correct angle
Vector2 newV1 = new Vector2(
u1.X * (float)Math.Cos(angle1) - u1.Y * (float)Math.Sin(angle1),
u1.X * (float)Math.Sin(angle1) + u1.Y * (float)Math.Cos(angle1));
Vector2 newV2 = new Vector2(
u2.X * (float)Math.Cos(angle2) - u2.Y * (float)Math.Sin(angle2),
u2.X * (float)Math.Sin(angle2) + u2.Y * (float)Math.Cos(angle2));
newV1 = new Vector2(
(float.IsNaN(newV1.X)) ? 0 : newV1.X,
(float.IsNaN(newV1.Y)) ? 0 : newV1.Y);
newV2 = new Vector2(
(float.IsNaN(newV2.X)) ? 0 : newV2.X,
(float.IsNaN(newV2.Y)) ? 0 : newV2.Y);
AddForce(newV1);
Object.Physik.AddForce(newV2);
}
bool checkCollidingObjects(IPhysicsObject obj1, IPhysicsObject obj2)
{
if (CollidingObjects.Count > 0)
{
int a = CollidingObjects.FindIndex(x => (x[0] == obj1 && x[1] == obj2) ||
(x[1] == obj1 && x[0] == obj2));
return a != -1;
}
return false;
}