Получение координаты в пространстве под углом на расстоянии от текущей координаты с поправкой pitch и roll

Вобщем тема такая

 

1я задачка была такая

Нужно получить координату на плоскости под углом на расстоянии от текущей координаты

Задачку решил тут

Код в итоге таков:

 

например выведем в лог  координаты точки на плоскости на расстоянии 2м и под углом 90 град (справа) от игрока (вызвав функцию PrintCoords, передав ей в качестве параметра переменную содержащую ссылку на перса игрока)

 

void PrintCoords(PlayerBase player)
{
  float distance = 2;
  float anglerotation = 90;
  Print("Coordinates on the plane at distance " + distance + " and at an angle relative to the player: " + anglerotation + ": " + GetCoords(player, distance, anglerotation).ToString());
}


vector GetGoords(PlayerBase player, float distance, float anglerotation)
{
	return GetPosFromDistAndAngle(player.GetPosition(), distance, player.GetOrientation()[0] + anglerotation);
}

vector GetPosFromDistAndAngle(vector pos, float dist, float angle)
{
	angle = Math.NormalizeAngle(angle) * Math.DEG2RAD;
	pos[0] = pos[0] + (double)(Math.Sin(angle) * dist); //offset X
	pos[2] = pos[2] + (double)(Math.Cos(angle) * dist); //offset Y
	return pos;
}

 

Это была задачка 1 и она была решена!

 

далее надо решить более сложную задачу - она будет номер 2

 

Надо вычислить координату не на плоскости, а вообще в пространстве, взяв в качестве поправок значения pitch и roll (наклон по поперечной и продольной осям) которые получаются из функции  обьект.GetOrientation() - первое значение масива типа вектор которое возвращает функция - угол на плоскости, второе угол в поперечной оси, третье в продольной (вроде так)
 

те если игрок, да не важно, просто обьект, имеет наклон в поперечной оси 45 градусов (от так как этот слэш /), то точка под углом на плоскости относительно игрока 90град и расстоянии 2м "справа для него будет 'ниже', чем слева"

 

врбщем -  нам надо получить координату под улом 90град на плоскости, 45град по вертикали и расстоянии 2м от него слева

 

воот

 

и тут у меня мозг взорвался (и почему-то захотелось вискаря)
 

я понимаю что в данном случае нужно отталкиваться от поперечной вертикальной плоскости... но чет не догоняю как........

 

 

Изменено 19 июля 2019 пользователем BorizzK (история изменений)

paranoyk я это в курсе

Но там вращение вокруг связанных осей вроде

Тож проходил это все лет 25 назад

 

Дык,сначала вопрот после ГЛОБАЛЬНОЙ оси,а потом уже вокруг тех которые у точки.

 

paranoyk ты про

х=х1+ D*sin A

y=y1+D*sin B
z= z+D*cos(√ A²+B²)

 

?

 

Читаю сейчас про кватернионы (как всегда на хабре)

https://m.habr.com/ru/post/255005/

Это то что нужно походу

Изменено 28 июля 2019 пользователем BorizzK (история изменений)

paranoyk 

 

х=х1+ D*sin A

y=y1+D*sin B
z= z+D*cos(√ A²+B²)

 

Давай разберем что есть что

 

z - координаты по оси Z - те высота/вертикаль?

 

углы A и B? Это Yaw и pitch? или?

 

	void mytest(vector pos, vector orientation, float angle, float dist)
	{
		float x, z, y;
		orientation[0] = orientation[0] + angle;
		х = pos[0] + dist * Math.Sin( orientation[0] );
		z = pos[1] + dist * Math.Cos( Math.Sqrt( Math.Pow(orientation[0],2) + Math.Pow(orientation[1],2) ) ); 
		y = pos[2] + dist * Math.Sin( orientation[1] );
		return Vector(x,z,y);
	}

 

BorizzK Имхо,поменять Z и Y....

А вообще,самый простой вариант...это определяем координаты на плоскости,плюсуем  нужную тебе высоту к "уровню земли"..

paranoyk 

к "уровню земли" = не вариант

нужно найти координату на прямой строго перпендикулярной обьекту в звисимости от его наклонов с той или иной стороны

 

 

vector TestFunc(vector pos, vector orientation, float dist, float angle) //angle это доворот на плоскости
	{
		float yaw = Math.NormalizeAngle(orientation[0] + angle) * Math.DEG2RAD; //довернули и в радианты
		float pitch = Math.NormalizeAngle(orientation[1]) * Math.DEG2RAD;
		float roll = Math.NormalizeAngle(orientation[2]) * Math.DEG2RAD;
		
		pos[0] = pos[0] + (double)(Math.Sin(yaw) * dist); //смещение по X
		pos[1] = pos[1] + (double)(Math.Sin(roll) * dist); //смещение по Z
		pos[2] = pos[2] + (double)(Math.Cos( Math.Sqrt( Math.Pow(yaw,2) + Math.Pow(roll,2) ) ) * dist); //смещение по Y
		return pos;
	}

Фигня получается

Изменено 29 июля 2019 пользователем BorizzK (история изменений)

BorizzK А нет ли в чудном языке бегемотов понятия "rotate"?

 

paranoyk там разное есть - но вот rotate как такового нет

в классе Math3D вот например

 

/**
 * \defgroup Math3DAPI Math3D library
 * @{
 */

/**
\brief Vector constructor from components
    \param x \p float x component
	\param y \p float y component
	\param z \p float z component
	\return \p vector resulting vector
	@code
		Print( Vector(1, 2, 3) );

		>> <1,2,3>
	@endcode
*/
proto native vector Vector(float x, float y, float z);

enum ECurveType
{
	CatmullRom,
	NaturalCubic,
	UniformCubic
};

class Math3D
{
	//-----------------------------------------------------------------
	static vector ClipLine(vector start, vector end, vector norm, float d)
	{
		vector vec;
		float d1, d2;

		d1 = start * norm;
		d1 = d1 - d;

		d2 = end * norm;
		d2 = d2 - d;

		d = d1 - d2;

		if(d1 < 0)
			d1 = d1 + 0.1;
		else
			d1 = d1 - 0.1;

		d = d1 / d;
		
		//clamp d<0, 1>
		if (d < 0) d = 0;
		if (d > 1) d = 1;

		vec = end - start;
		vec = vec * d;

		return vec + start;
	}

	proto static float IntersectRaySphere(vector raybase, vector raycos, vector center, float radius);
	proto static float IntersectRayBox(vector start, vector end, vector mins, vector maxs);
	
	/**
	\brief Creates rotation matrix from angles
		\param ang \p vector which contains angles
		\param[out] mat \p vector created rotation matrix
		@code
			vector mat[3];
			YawPitchRollMatrix( "70 15 45", mat );
			Print( mat );

			>> <0.330366,0.0885213,-0.939693>,<0.458809,0.854988,0.241845>,<0.824835,-0.511037,0.241845>
		@endcode
	*/
	proto static void YawPitchRollMatrix(vector ang, out vector mat[3]);

	/**
	\brief Creates rotation matrix from direction and up vector
		\param dir \p vector direction vector
		\param up \p vector up vector
		\param[out] mat \p vector[4] created rotation matrix
		@code
			vector mat[4];
			vector dir = "1 0 1";
			vector up = "0 1 0";
			DirectionAndUpMatrix( dir, up, mat );
			Print( mat );

			>> <0.707107,0,-0.707107>,<0,1,0>,<0.707107,0,0.707107>,<0,0,0>
		@endcode
	*/
	proto static void DirectionAndUpMatrix(vector dir, vector up, out vector mat[4]);

	/**
	\brief Transforms matrix
		\param mat0 \p vector[4] first matrix
		\param mat1 \p vector[4] second matrix
		\param[out] res \p vector[4] result of first and second matrix multiplication
		@code
			vector mat0[4] = { "2 0 0 0", "0 3 0 0", "0 1 0 0", "0 0 0 1" }; // scale matrix
			vector mat1[4] = { "1 0 0 0", "0 1 0 0", "0 1 0 0", "2 4 1 3" }; // translation matrix
			vector res[4];
			Math3D.MatrixMultiply4(mat0, mat1, res)
			Print( res );

			>> <2,0,0>,<0,3,0>,<0,3,0>,<4,13,0>
		@endcode
	*/
	proto static void MatrixMultiply4(vector mat0[4], vector mat1[4], out vector res[4]);

	/**
	\brief Transforms rotation matrix
		\param mat0 \p vector[3] first matrix
		\param mat1 \p vector[3] second matrix
		\param[out] res \p vector[3] result of first and second matrix multiplication
		@code
			vector mat0[3] = { "1.5 2.5 0", "0.1 1.3 0", "0 0 1" }; // rotation matrix
			vector mat1[3] = { "1 0.4 0", "0 1 0", "0 1.3 2.7" }; // rotation matrix
			vector res[3];
			Math3D.MatrixMultiply3(mat0, mat1, res)
			Print( res );

			>> <1.54,3.02,0>,<0.1,1.3,0>,<0.13,1.69,2.7>
		@endcode
	*/
	proto static void MatrixMultiply3(vector mat0[3], vector mat1[3], out vector res[3]);

	/**
	\brief Invert-transforms matrix
		\param mat0 \p vector[4] first matrix
		\param mat1 \p vector[4] second matrix
		\param[out] res \p vector[4] inverse result of first and second matrix multiplication
		@code
			vector mat0[4] = { "2 0 0", "0 3 0", "0 0 1", "0 0 0" }; // scale matrix
			vector mat1[4] = { "1 0 0", "0 1 0", "0 0 1", "2 4 1" }; // translation matrix
			vector res[4];
			Math3D.MatrixInvMultiply4(mat0, mat1, res)
			Print( res );

			>> <2,0,0>,<0,3,1>,<0,3,1>,<4,12,4>
		@endcode
	*/
	proto static void MatrixInvMultiply4(vector mat0[4], vector mat1[4], out vector res[4]);

	/**
	\brief Invert-transforms rotation matrix
		\param mat0 \p vector[3] first matrix
		\param mat1 \p vector[3] second matrix
		\param[out] res \p vector[3] result of first and second matrix multiplication
		@code
			vector mat0[3] = { "1.5 2.5 0", "0.1 1.3 0", "0 0 1" }; // rotation matrix
			vector mat1[3] = { "1 0.4 0", "0 1 0", "0 1.3 2.7" }; // rotation matrix
			vector res[3];
			Math3D.MatrixInvMultiply3(mat0, mat1, res)
			Print( res );

			>> <2.5,0.62,0>,<2.5,1.3,0>,<3.25,1.69,2.7>
		@endcode
	*/
	proto static void MatrixInvMultiply3(vector mat0[3], vector mat1[3], out vector res[3]);

	/**
	\brief Creates identity matrix
	    \param[out] mat \p created identity matrix
		@code
	        vector mat[4];
	    	Math3D.MatrixIdentity4( mat );
	    	Print( mat );

			>> <1,0,0>,<0,1,0>,<0,0,1>,<0,0,0>
		@endcode
	*/
	static void MatrixIdentity4(out vector mat[4])
	{
		mat[0] = "1 0 0";
		mat[1] = "0 1 0";
		mat[2] = "0 0 1";
		mat[3] = vector.Zero;
	}
	
	/**
	\brief Creates identity matrix
	    \param[out] mat \p created identity matrix
		@code
	        vector mat[3];
	    	Math3D.MatrixIdentity3( mat );
	    	Print( mat );

			>> <1,0,0>,<0,1,0>,<0,0,1>
		@endcode
	*/
	static void MatrixIdentity3(out vector mat[3])
	{
		mat[0] = "1 0 0";
		mat[1] = "0 1 0";
		mat[2] = "0 0 1";
	}

	/**
	\brief Creates scale matrix
	    \param scale \p scale coeficient
	    \param[out] mat \p created scale matrix
		@code
	        vector mat[3];
	    	Math3D.ScaleMatrix( 2.5, mat );
	    	Print( mat );

			>> <2.5,0,0>,<0,2.5,0>,<0,0,2.5>
		@endcode
	*/
	static void ScaleMatrix(float scale, out vector mat[3])
	{
		vector v0, v1, v2;
		v0[0] = scale;
		v1[1] = scale;
		v2[2] = scale;
		mat[0] = v0;
		mat[1] = v1;
		mat[2] = v2;
	}

	/**
	\brief Creates identity quaternion
		\param[out] q \p float[4] created identity quaternion
		@code
			float q[4];
			Math3D.QuatIdentity( q );
			Print( q );

			>> {0,0,0,1}
		@endcode
	*/
	static void QuatIdentity(out float q[4])
	{
		q[0] = 0;
		q[1] = 0;
		q[2] = 0;
		q[3] = 1;
	}

	/**
	\brief Copies quaternion
	    \param s \p float[4] quaternion to copy
		\param[out] d \p float[4] created quaternion copy
		@code
			float s[4] = { 2, 3, 4, 1 };
			float d[4];
			Math3D.QuatCopy( s, d );
			Print( d );

			>> {2,3,4,1}
		@endcode
	*/
	static void QuatCopy(float s[4], out float d[4])
	{
		d[0] = s[0];
		d[1] = s[1];
		d[2] = s[2];
		d[3] = s[3];
	}

	/**
	\brief Converts rotation matrix to quaternion
	    \param mat \p vector[3] rotation matrix
		\param[out] d \p float[4] created quaternion copy
		@code
			vector mat[3];
			vector rot = "70 15 45";
			rot.RotationMatrixFromAngles( mat );
			float d[4];
			Math3D.MatrixToQuat( mat, d );
			Print( d );

			>> {0.241626,0.566299,-0.118838,0.778973}
		@endcode
	*/
	proto static void MatrixToQuat(vector mat[3], out float d[4]);

	
	//! Converts quaternion to rotation matrix
	proto static void QuatToMatrix(float q[4], out vector mat[3]);

	/**
	\brief Returns angles of rotation matrix
		\param mat \p vector[3] rotation matrix
		\return \p vector roll, pitch, yaw angles
		@code
			vector mat[3];
			Math3D.RollPitchYawMatrix( "70 15 45", mat );
			vector ang = Math3D.MatrixToAngles( mat );
			Print( ang );

			>> <70,15,-45>
		@endcode
	*/
	proto static vector MatrixToAngles(vector mat[3]);
	
	/**
	\brief Linear interpolation between q1 and q2 with weight 'frac' (0...1)
	    \param[out] qout \p float[4] result quaternion
		\param q1 \p float[4] first quaternion
		\param q2 \p float[4] second quaternion
		\param frac \p float interpolation weight
		@code
			float q1[4] = { 1, 1, 1, 1 };
			float q2[4] = { 2, 2, 2, 1 };
			float qout[4];
			Math3D.QuatLerp( qout, q1, q2, 0.5 );
			Print( qout );

			>> {1.5,1.5,1.5,1}
		@endcode
	*/
	proto static void QuatLerp(out float qout[4], float q1[4], float q2[4], float frac);

	/**
	\brief Multiplies quaternions
	    \param[out] qout \p float[4] result quaternion
		\param q1 \p float[4] first quaternion
		\param q2 \p float[4] second quaternion
		@code
			float q1[4] = { 1, 2, 3, 1 };
			float q2[4] = { 2, 2, 2, 1 };
			float qout[4];
			Math3D.QuatMultiply( qout, q1, q2 );
			Print( qout );

			>> {2,4,6,1}
		@endcode
	*/
	proto static void QuatMultiply(out float qout[4], float q1[4], float q2[4]);

	//! Returns Angles vector from quaternion
	proto static vector QuatToAngles(float q[4]);
	
	/**
	\brief Returns 1, when bounding boxes intersects
	    \param mins1 \p vector minimum point of first bounding box
		\param maxs1 \p vector maximum point of first bounding box
		\param mins2 \p vector minimum point of second bounding box
		\param maxs2 \p vector maximum point of second bounding box
		\return \p int 1 if boundig boxes intersects, otherwise 0
		@code
			vector mins1 = "1 1 1";
			vector maxs1 = "3 3 3";
			vector mins2 = "2 2 2";
			vector maxs2 = "4 4 4";
			Print( Math3D.CheckBoundBox(mins1, maxs1, mins2, maxs2) );

			>> 1
		@endcode
	*/
	static int CheckBoundBox(vector mins1, vector maxs1,vector mins2, vector maxs2)
	{
		if (mins1[0] > maxs2[0] || mins1[1] > maxs2[1] || mins1[2] > maxs2[2] || maxs1[0] < mins2[0] || maxs1[1] < mins2[1] || maxs1[2] < mins2[2])
			return false;

		return true;
	}
	
	/**
	\brief Returns randon normalized direction
	    \return \p vector 
		@code
			Print( Math3D.GetRandomDir() );

			>>vector ret =  0x0000000007c1a1c0 {<0.422565,0,-0.906333>}
		@endcode
	*/
	static vector GetRandomDir()
	{
		float x = Math.RandomFloatInclusive(-1, 1);
		float y = Math.RandomFloatInclusive(-1, 1);
		float z = Math.RandomFloatInclusive(-1, 1);
			
		return Vector(x, y, z).Normalized();
	}
	
	
	/**
	\brief Computes curve
	 \return \p vector 
		@code
			auto points = new array<vector>();
			points.Insert( Vector( 0, 0, 0) );
	    	points.Insert( Vector( 5, 0, 0) );
	    	points.Insert( Vector( 8, 3, 0) );
	    	points.Insert( Vector( 6, 1, 0) );
		
			float t = 0.5;
			vector result = Math3D.Curve(ECurveType.CatmullRom, t, points);
		@endcode
	*/
	proto static native vector Curve(ECurveType type, float param, notnull array<vector> points);
};
//@}

 

вообще один из подходов в данном случае

находим точку на плоскости - собственно выше (в начале) я ее уже нашел

а дальше двигаем ее на угол roll - находим точку

и потом на pitch

 

уверен что есть проще методы

мы всегда можем получить direction

и его уже вращать

а потом полученный вектор умножить

 

 

123new 

GetDrection

Оно возвращает вектор прирощения относительно текущей ориентации в пространстве мира

Те если GetPosition сложить с GetDirection то обьект сместится в сторону

Как думаешь какую?

В сторону Orientation[1] причем его углы не поменяются

Те если задать object.SetOrientation("0 45 0")

И потом его object.SetPosition(object.GetPosition() + object.GetDirection())

То он продвинется вперед свеой ориентации чет там на полтора метра в напревлении наклона

Мне непонятно в каких там все единицах

Но факт в том что вот оно так

Сам проверь

И вращение

Если object.SetOrientation("45 0 0") - он повернется вокруг вертикальной оси - причем глобальной

А если object.SetOrientation("0 45 0") - вокруг оси  локальной X (поперечная ось) на 45 град мордом вверх

Прибавь Direction и оно сместится вверх по прямой под углом 45 град (набор высоты) - 90 град - строго вертикально

теперь если direction прибавлять он вертикально вверх двигаться будет те ось Z совпадет c его локальной Y

object.SetOrientation("0 0 45") - вокруг оси  локальной Y (продольная ось) - 90 на правый бок ляжет, -90 на левый

А если он вертикально object.SetOrientation("0 90 0") ???

 

А если object.SetOrientation("0 90 0")  то изменения yaw и roll для обьекта идентичны

те что yaw крути что roll - он крутится вокруг одной и той же оси

 

 

 

 

 

Изменено 29 июля 2019 пользователем BorizzK (история изменений)

BorizzK говоря простым языком, SetOrientation - поворот объекта относительно точки в формате x z y в пространстве, SetDirection - направление объекта как вектора направления относительно точки в формате x z y в пространстве. Не забываем, что z - высота в понимании у игры, а x и y - координаты на плоскости

123new Да

Но - поворот вокруг вертикальной оси Z - это всегда поворот вокруг глобальной оси

А вот повороты тела вокруг X Y всегда повороты вокруг локальных осей

Пока Orientation = 0 0 0 - то вращение 0 45 0 повернет вокруг оси X как локальной, так и глобальной, вращение 0 0 45 повернет вокруг оси Y как локальной так и глобальной, 45 0 0 вокруг Z, и вот после того как мы повернули 45 0 0 повороты 45 45 45 глобальных осей X Y не касаются, только локальных

 

и если обьект наклонен - его плоскость так же наклонена

 

ну и суть как бы не в этом

 

мне надо

 

независимо от Orientation обьекта

 

найти координаты точки относительно объекта под определенным углом к ЕГО плоскости и вектору направления на расстоянии

 

весь мозг уже сломал

 

пока решил так - временно

 

создаю другой обьект - шаблон

 

задаю ему поворот вокруг Z на нужный угол yaw который равен углу yaw эталонного обьекта + угол нужного мне доворота

затем переориентирую pitch - ориентирую фронт обьекта-шаблона углом roll эталонного обьекта - что бы его наклон был в направлении искомой точки

затем получаю direction, делю, складываю, умножаю - получаю приблизительные кординаты смещения

но это не работает корректно когда мне надо найти точки не точно под перпендикулярными углами, а по диагонали и тп

 

 

@BorizzK Здесь о механике спавна машин на сервере разговор ведется, правильно понимаю?)

fortnox2012 Нет - разговор о механике расчета

Получить координату точки на заданном расстоянии от обьекта под заданным углом зная координату положения обьекта и углы его ориентации (поворота в гск и наклонов в лск)

Изменено 30 июля 2019 пользователем BorizzK (история изменений)

Вобщем почитав и изучив немного вопрос пришел к выводу

Все эти синусы и косинусы хороши в нашем случае только для получения координат на плоскости

 

А вот вообще в пространстве

Нужно взять вектор направления, повернуть его на 2 угла и получив новый вектор направления просто сделать ему приращение методом умножения на дистанцию и суммировать с текущими координатами

Все

 

Осталось решить как вращать этот вектор

И главное как? Написать расчет самостоятельно - дичь выходит

Но ведь есть штатные функции в Math3D

осталось понять как ими пользоваться...

 

Времени не было ковырять

Так - слегка поковыривал

В итоге сделал костыль временный - через создание обьекта в vector.Zero, его переориентацию в зависимости от углов и получения Direction, а затем методом сложения direction и нужной позиции получил нужную координату, однако немного все криво в случае больших углов наклона исходного обьекта/игрока

 

Порыл - нашел функцию AnglesToVector()

 

Поробовал

orientation[0] = orientation[0] + angle;
vector new_dir = orientation.AnglesToVector() * dist;
return pos + new_dir;

 

работает - но опять же - на плоскости, не поднимает угол ((( хотя по логике должна....

 

ибо

 

 /**
 \brief Converts spherical coordinates (yaw, pitch, roll in degrees) to unit length vector

 

но есть нюанс
  \return \p normalized direction vector

 

походу на плоскости именно из-за нормализации....

 

 

В 31.07.2019 в 10:31, BorizzK сказал:

Все эти синусы и косинусы хороши

В 25.07.2019 в 13:08, NoNameUltima сказал:

хочешь с синусами косинусами заморачивайся, или бери квантерионы эйлера, и векторку.

Еще пару недель, и всежтаки до квантиков в векторке доберешся))))

NoNameUltima не - не доберусь

я про них читал перечитал

как и про матрицы

 

не врубаюсь от слова совсем

 

озадачил кучу народа которые в этом понимают

накарябали разного кода на C# и тп

я переписал в enfusion

 

НЕ РАБОТАЕТ - точнее делает не то и не так

 

при этом в EnMath3D есть горка функций - получение матриц, квантернионов из углов и тп

но как выходные данные юзать ровным счетом непонятно

 

сегодня задал вопрос богемии

 

посмотрим что ответят

Ну, что, никто не готов присоединиться к поиску решения?

12 часа назад, BorizzK сказал:

Ну, что, никто не готов присоединиться к поиску решения?

рад бы даже может, братишка, но мозгов выносят мне самому с начала года капитально так, то ли организм думать отказывается, толи морально устаю за день.  Да и дело наскока понимаю не тока в логике программирования, задачу твою сначала надо последовательно обрисовать на бумаге, так сказать. составить условную блок-схему, и уже по ней составить что и из чего надо просчитывать в какие велечины, и потом обрисовывать уж законами игры. Ну это как мне видится для простоты понимания.

123new Ну как бы я уже - нашел что надо - однако - обьект вращается не вокруг своего центра - а вокруг своего положения

Игрок - вокруг координаты в ногах, авто вокруг координаты которая под днищем сантиметров на 15 ниже - те если ставить горизонтально то уровень колес

Те надо еще и точку по дуге вращать

Пока

Вот такое решение -

float distance = 0.5; длина (расстояние)

float angle = 45; - угол поворота на плоскости

orientation[0] = orientation[0] + angle; //повернули YAW

vector new_dir = orientation.AnglesToVector(); //получаем вектор направления в зависимости от YAW - pitch и roll игнориуются движком

vector new_dir_Up = "0 0 0";

vector new_dir_Up[0] = orientation[2]; //(загоняем ROLL в YAW)

new_dir_Up = new_dir_Up.AnglesToVector(); //Оно нам расчитает вектор в YAW который мы используем для задания вертикального направления

new_dir[1] = new_dir_Up[0];

vector new_pos = pos + (new_dir * dist);

 

получили максимально приближенное к реальности (ну там + - пара см роли не играет)

 

ну а смещения для исходной точки нашел опытным путем через угол и его косинус, синус + поправки