Relative position and finding an angle (in 3d)

[vitovc]

I'd wanted to share some functions from my vc-mp archive.

This function will find relative position based by z_angle (vertical angle) and relative X and Y distance from base point.

Code Select Expand

rel_pos2d <- function(z_angle, pos_x, pos_y, pos_z, rel_x, rel_y, rel_z){
  local rX = pos_x + rel_x * ::cos(z_angle) - rel_y * ::sin(z_angle);
  local rY = pos_y + rel_x * ::sin(z_angle) + rel_y * ::cos(z_angle);
  local rZ = pos_z + rel_z;
  return ::Vector(rX, rY, rZ);
}
This function will find relative position based by z_angle (vertical angle), y_angle (horizontal angle) and distance.

Code Select Expand

rel_pos3d <- function(dist, pos_x, pos_y, pos_z, y_angle, z_angle){
  local rX = dist * ::cos(y_angle) * ::cos(z_angle);
  local rY = dist * ::cos(y_angle) * ::sin(z_angle);
  local rZ = dist * ::sin(y_angle);
  return ::Vector(pos_x + rX, pos_y + rY, pos_z + rZ);
}
Update: And some reverse functions to find angle.

This function will find horizontal angle (z axis).

Code Select Expand

get_angle <- function(x, y, x2, y2){
  return ::atan2(y2 - y, x2 - x);
}
This function will find vertical angle.

Code Select Expand

dist_2d <- function (x, y, x2, y2){
  return ::sqrt((x - x2)*(x - x2) + (y - y2)*(y - y2));
}
get_vangle <- function(x1, y1, z1, x2, y2, z2){
  return ::atan2(z2 - z1, ::dist_2d(x2,y2,x1,y1));
}
After angle found you may want to add/extract some value to this angle and then use it. Before that you need to normalize angle by this function.

Code Select Expand

normalize_angle <- function (a){
  return ::atan2(::sin(a), ::cos(a));
}

[PSL]

Thank you for sharing

[vitovc]

updated; added finding angle functions