Calculating Angle between two Lines in 3d Space using IMU data & Trigonometry
in
Programming Questions
•
2 months ago
I have 4 points in 3d space: A,B,C,D
These are connected by three lines AB, BC and CD.
What I need:
Yaw and Roll of CD relative to BC
What I know:
Points A,C
What I know:
Points A,C
Pitch Yaw and Roll of AB and BC
Distance AB and BC
The problem:
I am not really sure. I almost have it all put together, but I get weird artifacts when drawing visualizations. Little things are not behaving as they should (i.e. I believe a line should be horizontal, the numeric output is horizontal, but processing draws it with a slight incline). The problem is that it part of a bigger project and I am having trouble locating the problem. I would like to be able to make sure that my math is solid. If anyone could help me think through this, it would help me a lot.
I will not post the whole code, as I think it might be overkill, but start with the parts which are most critical.
I am not really sure. I almost have it all put together, but I get weird artifacts when drawing visualizations. Little things are not behaving as they should (i.e. I believe a line should be horizontal, the numeric output is horizontal, but processing draws it with a slight incline). The problem is that it part of a bigger project and I am having trouble locating the problem. I would like to be able to make sure that my math is solid. If anyone could help me think through this, it would help me a lot.
I will not post the whole code, as I think it might be overkill, but start with the parts which are most critical.
(I am happy to post the rest, if it helps, but at the moment what is most important to me is to know that the following functions are doing what I think they are.)
Step1 - Calculate B
StartingPoint = A
BONE_LENGTH = Distance AB
Endpoint = B
- PVector CalculateEndPoint()
- {
- float endpointY = sin(rotation.y)*BONE_LENGTH;
- //cos(pitch) = adjecant / hypothenuse
- //projection is the line AB projected onto the 'ground plane'
- float projectionXZ = cos(rotation.y)*BONE_LENGTH;
- //sin(yaw) = opposite / hypotenuse
- float endpointZ = cos(rotation.x)*projectionXZ;
- // float eX = sqrt((projectionXZ*projectionXZ)-(eZ*eZ));
- float endpointX = sin(rotation.x)*projectionXZ;
- endPoint.set(eX+startingPoint.x, eY+startingPoint.y, eZ+startingPoint.z);
- return endPoint;
- }
Step2 - Calculate C
Using the same method as above, jut this time:
StartingPoint = B
BONE_LENGTH = Distance BC
Endpoint = C
PVector C & D are simply the coordinates of their respective points
(D was given and C I calculated in steps 1 and 2)
Yaw is BCs absolute yaw angle, which i given
- float GetYaw (PVector C, float yaw BC, PVector D) {
- return = yaw+ (atan((D.x - C.x)/(D.z - C.z)));
- }
What I want out of the angle between BC and CD projected down on the floor plane.
Step3b - Calculate difference in roll of BC and pich CD
PVector C & D are simply the coordinates of their respective points
(D was given and C I calculated in steps 1 and 2)
Roll is BCs absolute roll angle
- float GetRoll (PVector C, float rollBC, PVector D) {
- return relativeRotation = rollBC- atan((D.y - C.y)/(D.z - C.z));
- }
I know its sort of wierd talking of roll in this situation. In order to better conceptualize what I want to achieve here, imagine there is a camera hanging on the wall. It is filming an analogue watch which is hanging, somewhat offset, on the oppoite wall. I want to know the angle between the minute pointer and the center of the image.
The center of the image would be my point D. The angle which the minute pointer is at would be my Roll angle.
(and I only know where this imaginary clock is, because I know its at the end of line BC)
Is it clear what I am trying to do here?
Is it clear what I am trying to do here?
*
Does my logic make sense?
Does my logic make sense?
1