Develop Reference

3D-Coordinate Transformation in C#

What is the Goal?:
I want to know the new Coordinates of a point after rotating the 3D-Object (Cuboid), around the anchorpoint (x,y & z) on the opposite side.
What i did:
I tried to calculate the position with the following function. I had to convert doubles to floats , because of the Autodesk Inventor API. Note: Vector is the difference from the origin /anchorpoint to the designated point.
Vector3 coordinateTransformation(Vector3 vector, float r_x, float r_y, float r_z, Vector3 origin)
{
vector.X = vector.X; //Just for demonstration
vector.Y = vector.Y * Convert.ToSingle(Math.Cos(DegreesToRadians(r_x))) - vector.Z * Convert.ToSingle(Math.Sin(DegreesToRadians(r_x)));
vector.Z = vector.Y * Convert.ToSingle(Math.Sin(DegreesToRadians(r_x))) + vector.Z * Convert.ToSingle(Math.Cos(DegreesToRadians(r_x)));
vector.X = vector.X * Convert.ToSingle(Math.Cos(DegreesToRadians(r_y))) + vector.Z * Convert.ToSingle(Math.Sin(DegreesToRadians(r_y)));
vector.Y = vector.Y; //Just for demonstration
vector.Z = vector.Z * Convert.ToSingle(Math.Cos(DegreesToRadians(r_y))) - vector.X * Convert.ToSingle(Math.Sin(DegreesToRadians(r_y)));
vector.X = vector.X * Convert.ToSingle(Math.Cos(DegreesToRadians(r_z))) - vector.Y * Convert.ToSingle(Math.Sin(DegreesToRadians(r_z)));
vector.Y = vector.X * Convert.ToSingle(Math.Sin(DegreesToRadians(r_z))) + vector.Y * Convert.ToSingle(Math.Cos(DegreesToRadians(r_z)));
vector.Z = vector.Z; //Just for demonstration
vector.X = Math.Abs(vector.X) + origin.X;
vector.Y = Math.Abs(vector.Y) + origin.Y;
vector.Z = Math.Abs(vector.Z) + origin.Z;
return vector;
}
Somehow the object does not get placed on the correct place.
Next step: On the internet i found a website which does the correct transformation.Casio Website
If i manually set vector to the calculated point on the website, everything else works fine. So i somehow have to get the exact same calculation into my code.
If you need further information, feel free to comment!
Edit:
1 : I want to place 2 Objects (e.g. Cuboids) within 1 Assembly Group in Inventor. Every Object as an anchorpoint (origin) and on the opposite side a connection point, which is described as the delta between the anchorpoint and the connection point itself. At first one Object is placed on the origin coordinates, followed by a rotation around the anchorpoint (degrees). After that the connection point coordinates of Object 1 have changed. In the next step i want to place Object 2 with its origin on the connection point of Object 1, while Object 2 has the same rotation as Object 1.
2 : Inventor uses a right-handed coordinate system

When you apply rotation to a vector manually, you'd need to update all the components (X, Y and Z) at once as follows.
Vector3 coordinateTransformation(Vector3 vector, float r_x, float r_y, float r_z, Vector3 origin)
{
// In the rotation around X axis below, `Y relies on Z` and `Z relies on Y`. So both must be updated simultaneously.
vector = new Vector3(
vector.X,
vector.Y * Convert.ToSingle(Math.Cos(DegreesToRadians(r_x))) - vector.Z * Convert.ToSingle(Math.Sin(DegreesToRadians(r_x))),
vector.Y * Convert.ToSingle(Math.Sin(DegreesToRadians(r_x))) + vector.Z * Convert.ToSingle(Math.Cos(DegreesToRadians(r_x)))
);
vector = new Vector3(
vector.X * Convert.ToSingle(Math.Cos(DegreesToRadians(r_y))) + vector.Z * Convert.ToSingle(Math.Sin(DegreesToRadians(r_y))),
vector.Y,
vector.Z * Convert.ToSingle(Math.Cos(DegreesToRadians(r_y))) - vector.X * Convert.ToSingle(Math.Sin(DegreesToRadians(r_y)))
);
vector = new Vector3(
vector.X * Convert.ToSingle(Math.Cos(DegreesToRadians(r_z))) - vector.Y * Convert.ToSingle(Math.Sin(DegreesToRadians(r_z))),
vector.X * Convert.ToSingle(Math.Sin(DegreesToRadians(r_z))) + vector.Y * Convert.ToSingle(Math.Cos(DegreesToRadians(r_z))),
vector.Z
);
vector.X = Math.Abs(vector.X) + origin.X;
vector.Y = Math.Abs(vector.Y) + origin.Y;
vector.Z = Math.Abs(vector.Z) + origin.Z;
return vector;
}
Appendix: Use of Matrix4x4
As #JonasH suggests, it's a good idea to use reliable libraries, if there are.
Since, row-vectors are used in the .NET world (while your implementation is based on column-vectors), X->Y->Z rotations can be written by straightforward matrices multiplications as follows.
Vector3 coordinateTransformation(Vector3 vector, float r_x, float r_y, float r_z, Vector3 origin)
{
var mat = Matrix4x4.CreateRotationX(DegreesToRadians(r_x))
* Matrix4x4.CreateRotationY(DegreesToRadians(r_y))
* Matrix4x4.CreateRotationZ(DegreesToRadians(r_z));
vector = Vector3.Transform(vector, mat);
vector.X = Math.Abs(vector.X) + origin.X;
vector.Y = Math.Abs(vector.Y) + origin.Y;
vector.Z = Math.Abs(vector.Z) + origin.Z;
return vector;
}

Do not mess around with vectors and angled by hand. Use some library for your vector functions and use matrices to do your transformations. For example System.Numerics or Math.Net.Numerics.
Assuming you have an euler angle to start with you can use CreateFromYawPitchRoll to create your rotation matrix. If you want to rotate around a specific point you just subtract your rotation center from all your points, apply the rotation and move the points back. This can all be done by just multiplying the transformation matrices:
var totalTransform = Matrix4x4.CreateTranslation(-rotationCenter) *
Matrix4x4.CreateFromYawPitchRoll(yawInRadians, pitchInRadians, rollInRadians) *
Matrix4x4.CreateTranslation(rotationCenter);
to apply the resulting transform you just call Vector3.Transform to produce a new, transformed vector.
This will all be a bit easier if you have some introduction to linear algebra. It is also common to do things like applying the transforms in the wrong order or something, and sometimes it is easiest to just try some different things until you get it right. I would also note that rotations are just hard to understand. I tend to prefer quaternions over eutler angles, not because the math is easier to understand, but since there are methods like CreateFromAxisAngle, that I can understand.

Angle offsets and positive/negative angles

I've been stuck with rotating an asset with an offset.
Sstart = e.GetPosition(dial);
if (dial.IsStylusCaptured)
{
AngleRot = Math.Atan2((Y - Sstart.Y) , (X - Sstart.X));
radAngle = AngleRot / Math.PI * 180 + 180;
radAngle = radAngle - AthetaD;
di.RenderTransform = new RotateTransform(radAngle + 90);
}
Using this I was able to rotate my object from 0° with an offset of x to angle theta. But when I make a second rotation, instead of it rotating from angle theta with an offset x it resets the object back to 0°. How can I make it so the offset is always from theta and not 0°?
Here I Rotate with an offset angle
Here my angle resets back to 0° instead of moving from -56°

Let current direction vector is (cx, cy).
In the beginning set it into (1,0) or another needed value.
When you rotated dial, you have new direction
nx = X - Sstart.X
ny = Y - Sstart.Y
To rotate current direction vector to new one, you need to calculate relative angle. This approach uses cross product and dot product of vectors. Perhaps you'll need to change sign of the first expression
Angle = Math.Atan2(nx * cy - ny * cx, cx * nx + cy * ny)
..apply rotation by Angle
After rotation remember current direction to use it later
cx = nx
cy = ny

The angle sin returns a negative result for the acute angle

I calculate the angles of a triangle, and I don't understand why I get a negative angle for some acute angle. For example:
var sin = Math.Sin(4.45);
var radians = Math.Atan(sin);
var angle = radians * (180 / Math.PI);
it return sin = -0.965 and angle = -44.
When scientific calculator show sin = 0.0775
My triangle has such lengths 6.22, 6.07 and 1.4 then there isn't option to had negative angle.

Math.Sin operates on radians. You need to convert degrees into radians.
To convert degrees to radians multiply the angle by 𝜋/180:
var sin = Math.Sin(4.45*Math.PI/180);
// output 0.07758909147106598
And the rest of your code should remain the same.
Note: if you just want to convert an angle in degrees to angle in radians you can use the formula above:
var degrees = 4.45;
var radians = degrees * Math.PI/180;

Let's compute angles of the triangle with a help of Law of cosines:
a**2 + b**2 - 2 * a * b * cos(gamma) == c**2
so
gamma = acos((a * a + b * b - c * c) / (2 * a * b))
beta = acos((a * a + c * c - b * b) / (2 * a * c))
alpha = acos((c * c + b * b - a * a) / (2 * c * b))
now put triangle lengths
a = 6.22
b = 6.07
c = 1.40
into formulae above and you'll get angles (in radians, if you use c# Math.Acos)
alpha = 1.5639 = 89.6 degrees
beta = 1.3506 = 77.4 degrees
gamma = 0.2270 = 13.0 degrees
------------------------------
180.0 degrees (let's check ourselves)
Another check is Law of sines
a / sin(alpha) == b / sin(beta) == c / sin(gamma) == 6.2201

Eigen Point-to-Plane algorithm, return quaternions?

I am using Eigen to calculate the best fit of a set of points to a plane. What I need to do with this data, is then rotate the set of points so they lie flat, negating the rotation value.
My code is:
cv::Point2f plane_from_points(const std::vector<Vector3> & c)
{
// copy coordinates to matrix in Eigen format
size_t num_atoms = c.size();
Eigen::Matrix< Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic > coord(3, num_atoms);
for (size_t i = 0; i < num_atoms; ++i) coord.col(i) = c[i];
// calculate centroid
Vector3 centroid(coord.row(0).mean(), coord.row(1).mean(), coord.row(2).mean());
// subtract centroid
coord.row(0).array() -= centroid(0); coord.row(1).array() -= centroid(1); coord.row(2).array() -= centroid(2);
// we only need the left-singular matrix here
// http://math.stackexchange.com/questions/99299/best-fitting-plane-given-a-set-of-points
auto svd = coord.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
Vector3 plane_normal = svd.matrixU().rightCols<1>();
float x = plane_normal[0];
float y = plane_normal[1];
float z = plane_normal[2];
float angle = atan2(x, z) * 180 / PI;
float angle2 = atan2(y, z) * 180 / PI;
cv::Point ret(angle, angle2);
return ret;
}
Then, in C#, I convert the angle values to a quaternion, to rotate my object:
public static Quaternion QuatFromEuler(double yaw, double pitch, double roll)
{
yaw = Deg2Rad(yaw);
pitch = Deg2Rad(pitch);
roll = Deg2Rad(roll);
double rollOver2 = roll * 0.5f;
double sinRollOver2 = (double)Math.Sin((double)rollOver2);
double cosRollOver2 = (double)Math.Cos((double)rollOver2);
double pitchOver2 = pitch * 0.5f;
double sinPitchOver2 = (double)Math.Sin((double)pitchOver2);
double cosPitchOver2 = (double)Math.Cos((double)pitchOver2);
double yawOver2 = yaw * 0.5f;
double sinYawOver2 = (double)Math.Sin((double)yawOver2);
double cosYawOver2 = (double)Math.Cos((double)yawOver2);
Quaternion result = new Quaternion();
result.W = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.X = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.Y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
result.Z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
return result;
}
This gives me:
angles: -177 -126
quat: -0.453834928533952,-0.890701198505913,-0.0233238317256566,0.0118840858439476
Which, when i apply it, looks nothing like it should. (I expect a roughly 45 degree rotation in one axis, I get a 180 degree flip)
I have tried switching the axes to check for coordinate space mismatch(which is likely), but I cannot get this to work. Am I doing something wrong?
I have checked the 3d points that i pass into the algorithm, and they are correct, so my issue is either in the point-to-plane code, or the quaternion conversion.
Any help would be much appreciated. Thank you.

If you want to calculate the quaternion which rotates one plane to another, simply compute the quaternion that rotates the normal to the other:
#include <Eigen/Geometry>
int main() {
using namespace Eigen;
// replace this by your actual plane normal:
Vector3d plane_normal = Vector3d::Random().normalized();
// Quaternion which rotates plane_normal to UnitZ, or the plane to the XY-plane:
Quaterniond rotQ = Quaterniond::FromTwoVectors(plane_normal, Vector3d::UnitZ());
std::cout << "Random plane_normal: " << plane_normal.transpose() << '\n';
std::cout << "rotated plane_normal: " << (rotQ * plane_normal).transpose() << '\n';
}
Also, don't store your angles in degrees, ever (it may sometimes make sense to output them in degrees ...).
And more importantly: Stop using Euler Angles!

Gravity and Centripetal Force

I am making an application in Unity 3d where I want to code gravity and centripetal force myself but I am getting weird results, am I doing it right? This is my code:
public void Attract(MassObject[] _allMass)
{
Vector3 F = new Vector3();
Vector3 C = new Vector3();
foreach(MassObject i in _allMass)
{
// gravity pull
F.x = GV.gravity * ((mass * i.mass) / (obj.position.x - i.obj.position.x));
F.y = GV.gravity * ((mass * i.mass) / (obj.position.y - i.obj.position.y));
F.z = GV.gravity * ((mass * i.mass) / (obj.position.z - i.obj.position.z));
// centripital force
C.x = (mass * Mathf.Pow(vel.x,2)) / (obj.position.x - i.obj.position.x);
C.y = (mass * Mathf.Pow(vel.y,2)) / (obj.position.y - i.obj.position.y);
C.z = (mass * Mathf.Pow(vel.z,2)) / (obj.position.z - i.obj.position.z);
}
vel = F + C;
Debug.Log(F);
Debug.Log(C);
}

There are a few problems with this:
You are rewriting F and C with each foreach iteration, so you end up with the for for the last object in the list. You should add all the forces together using += instead of =.
You should be using vector operations rather than manually calculating x y and z. Doing so would make the third error more obvious:
The formula is wrong. For example, this line:
F.x = GV.gravity * ((mass * i.mass) / (obj.position.x - i.obj.position.x));
with just a few things substituted is
F.x = gravity coeff * (masses multiplied) / (distance **on the x axis** between objects.)
What you really need is distance between objects, not just on the x axis. You can get this with a vector operation:
float distanceSquared = (obj.position - i.obj.position).sqrMagnitude;
Then you can use the law of universal gravitation to get the magnitude of the force of gravity:
float forceMagnitude = GV.gravity * (mass * i.mass) / distanceSquared;
Finally, you need a direction. You can get that with vector subtraction and normalization, then multiplying by the desired magnitude:
Vector3 force = (i.obj.position - obj.position).normalized * forceMagnitude;
And now force is a vector pointing from obj to i.obj with the magnitude of gravity between the two objects. Add that to F to accumulate the force from each object and things should be good.
Your understanding of physics might be off. If vel is velocity, it is wrong to set it to the sum of two forces. I think you're better off learning about this from somewhere else, though. Understanding Newton's laws is a good start.