VR 콘텐츠 제작
디스크 골프 [#2] - 프리스비 운동 R&D
송현호
2023. 11. 9. 18:11
프리스비 시뮬레이터의 파이썬 코드를 C#으로 바꾸고 전체적인 내용을 봤는데 Chat gpt로 바꾼 코드는 흐름을 파악하기엔 좋았지만 실제로 적용하기엔 좀 문제인 부분이 많았다. 그래서 C#의 코드를 참조하되 파이썬의 코드와 같이 R&D를 진행하기로 했다.
힘과 토크를 계산하는 메서드에서 가장 먼저 실행되는 코드는 Result의 딕셔너리를 만들고 ComputeAngleOfAttack메서드로 공기역학에서 사용되는 A각을 계산하는 메서드이다.
public static (float, float, float, float, float, Matrix4x4, float, Vector3) ComputeAngleOfAttack(float phi, float theta, Vector3 velocity, bool returnAllVariables = false)
{
float sp = Mathf.Sin(phi);
float cp = Mathf.Cos(phi);
float st = Mathf.Sin(theta);
float ct = Mathf.Cos(theta);
Matrix4x4 rotationMatrix = RotationMatrix(sp, cp, st, ct);
Vector3 zhat = rotationMatrix.GetColumn(2);
float vDotZhat = Vector3.Dot(velocity, zhat);
Vector3 vInPlane = velocity - zhat * vDotZhat;
float angleOfAttack = -Mathf.Atan2(vDotZhat, vInPlane.magnitude);
if (returnAllVariables)
{
return (angleOfAttack, sp, cp, st, ct, rotationMatrix, vDotZhat, vInPlane);
}
else
{
return (angleOfAttack, 0, 0, 0, 0, Matrix4x4.identity, 0, Vector3.zero);
}
}
private static Matrix4x4 RotationMatrix(float sp, float cp, float st, float ct)
{
Matrix4x4 matrix = new Matrix4x4();
matrix.SetColumn(0, new Vector4(ct, 0, st, 0));
matrix.SetColumn(1, new Vector4(sp * st, cp, -st * cp, 0));
matrix.SetColumn(2, new Vector4(-st * cp, -sp * st, cp * ct, 0));
matrix.SetColumn(3, new Vector4(0, 0, 0, 1));
return matrix;
}
RotationMatrix에 대해서 chat gpt에게 물어보았다.
설명만 들어선 정확하게 이해가 안되서 Rotation Matrix(회전 행렬)에 대해서 검색해보았다.
[동역학] 회전 변환 행렬(2D & 3D)
이번 포스팅에서는 회전 변환 행렬에 대해 알아봅시다. 회전 변환 행렬 (rotation matrix) 회전 변환 행렬이란, 좌표계에서 회전 변환을 할 때 사용하는 행렬을 말합니다. 2차원 직교좌표계에서 θ만
study2give.tistory.com
원문 코드의 주석을 통해 이해를 해보면 랩 프레임에서 디스크프레임으로 부분적인 행렬변환을 실행하고
zhat은 z축 방향으로의 단위벡터
v_dot_zhat은 속도와 zhat을 내적한 값
v_in_plain은 내적한 값과 zhat을 곱한 값을 속도에서 뺀다음 평면에서의 속도를 구한 값이다.
이를 바탕으로 Angle of Attack을 구하고 있다.
코드를 보니 x축과 y축을 평면으로 z축을 높이로 하고 있기에 나중에 이 점을 유의해서 바꿔줘야 할 것 같다.
public Dictionary<string, object> ComputeForcesAndTorques(float phi, float theta, Vector3 velocity, Vector3 angularVelocity)
{
Dictionary<string, object> result = new Dictionary<string, object>();
var angleOfAttackResult = ComputeAngleOfAttack(phi, theta, velocity, true);
float angleOfAttack = angleOfAttackResult.Item1;
float sp = angleOfAttackResult.Item2;
float cp = angleOfAttackResult.Item3;
float st = angleOfAttackResult.Item4;
float ct = angleOfAttackResult.Item5;
Matrix4x4 rotationMatrix = angleOfAttackResult.Item6;
float vDotZhat = angleOfAttackResult.Item7;
Vector3 vInPlane = angleOfAttackResult.Item8;
Vector3 zhat = rotationMatrix.GetColumn(2);
Vector3 xhat = vInPlane.normalized;
Vector3 yhat = Vector3.Cross(zhat, xhat);
float forceAmplitude = force_per_v2 * velocity.sqrMagnitude;
// Lift force
Vector3 F_lift = model.C_lift(angleOfAttack) * forceAmplitude * Vector3.Cross(velocity.normalized, yhat);
// Drag force
Vector3 F_drag = model.C_drag(angleOfAttack) * forceAmplitude * (-velocity.normalized);
// Gravitational force
Vector3 F_grav = this.F_grav;
// Total force
Vector3 F_total = F_lift + F_drag + F_grav;
// Acceleration
Vector3 acceleration = F_total / mass;
result.Add("F_lift", F_lift);
result.Add("F_drag", F_drag);
result.Add("F_grav", F_grav);
result.Add("F_total", F_total);
result.Add("Acceleration", acceleration);
// Torque
float torqueAmplitude = torque_per_v2 * velocity.sqrMagnitude;
Vector3 w = new Vector3(angularVelocity.x * ct, angularVelocity.y, angularVelocity.x * st + angularVelocity.z);
Vector3 T_x_lab = model.C_x(w.x, w.z) * torqueAmplitude * xhat;
Vector3 T_y_lab = model.C_y(angleOfAttack, w.y) * torqueAmplitude * yhat;
Vector3 T_z = model.C_z(w.z) * torqueAmplitude * z_hat;
// Rotate into the disc frame
Vector3 T_x = rotationMatrix * T_x_lab;
Vector3 T_y = rotationMatrix * T_y_lab;
// Total torque
Vector3 T_total = T_x + T_y + T_z;
result.Add("T_x_lab", T_x_lab);
result.Add("T_y_lab", T_y_lab);
result.Add("T_z", T_z);
result.Add("T_x", T_x);
result.Add("T_y", T_y);
result.Add("T_total", T_total);
return result;
}
zhat이 z축으로의 단위 벡터였기에 말할것도 없이 yhat xhat도 같은 역할이고
forceAmplitude는 1/2 x 공기밀도 x 반경 x 속도의 제곱을 곱한 값이다.
드래그와 리프트 포스는 이전에 작성했던 것과 같은 공식을 사용한다.
torqueAmplitude는 forceAmplitude에 Diameter(프리스비의 직경)을 곱한 값이다.
w는 회전속도
나머지는 변수명처럼 토크값을 의미한다.
코드를 보고 있자니 AddForce와 AddTorque를 사용했을때 AddTorque의 값이 과연 진행 방향에 반영될까라는 의문점이 들었다. 그래서 간단하게 코드를 작성해서 실험해보기로 했다.
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class Frisbee : MonoBehaviour
{
private Rigidbody rb;
void Start()
{
rb = GetComponent<Rigidbody>();
this.rb.AddForce((Vector3.forward + Vector3.up) * 1500f);
this.rb.AddTorque(Vector3.up * 500f);
}
private void FixedUpdate()
{
}
}
음 원반을 Y축으로 회전하게 시켰는데도 불구하고 X축이 변하지 않는 걸 보니 AddTorque를 해도 물체의 회전에만 영향을 줄뿐 진행방향에는 영향을 주지 않는다는걸 알 수 있었다.
다만 AddForce를 할떄 계산하는 값이 Torque의 영향을 받으므로 AddForce를 사용하거나 리지드바디를 배제하고 시뮬레이터처럼 값을 작성해야하나 라는 생각이 들었는데 일단은 AddForce를 사용하는 방향으로 코드를 작성해보기로 하였다.
계수의 유도는 Model 스크립트를 활용하기로하고 Frisbee스크립트를 만들어서 유니티환경에 적용할 수 있는 코드를 만들어보기로했다.
일단 phi값과 theta값을 구해야하기 때문에 Debug로 물체의 로테이션을 출력할 수 있는지 찍어보았다.
using MathNet.Numerics.Distributions;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class Frisbee : MonoBehaviour
{
private Rigidbody rb;
void Start()
{
rb = GetComponent<Rigidbody>();
this.rb.AddForce((Vector3.forward + Vector3.up) * 1500f);
this.rb.AddTorque(Vector3.up * 500f);
}
private void FixedUpdate()
{
float phi = this.transform.rotation.x;
float theta = this.transform.rotation.z;
Debug.Log(RotationMatrixFromPhiTheta(phi, theta));
}
public static Matrix4x4 RotationMatrixFromPhiTheta(float phi, float theta)
{
float sp = Mathf.Sin(phi);
float cp = Mathf.Cos(phi);
float st = Mathf.Sin(theta);
float ct = Mathf.Cos(theta);
return RotationMatrix(sp, cp, st, ct);
}
private static Matrix4x4 RotationMatrix(float sp, float cp, float st, float ct)
{
Matrix4x4 matrix = new Matrix4x4();
matrix.SetColumn(0, new Vector4(ct, 0, st, 0));
matrix.SetColumn(1, new Vector4(sp * st, cp, -st * cp, 0));
matrix.SetColumn(2, new Vector4(-st * cp, -sp * st, cp * ct, 0));
matrix.SetColumn(3, new Vector4(0, 0, 0, 1));
return matrix;
}
}
그리고 코드를 작성해보았다.
using MathNet.Numerics.Distributions;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class Frisbee : MonoBehaviour
{
private Rigidbody rb;
private float area;
private float diameter;
private float air_density;
private Model model;
private float force_per_v2;
private float torque_per_v2;
private Vector3 z_hat;
void Start()
{
this.area = 0.0568f;
this.diameter = 2 * Mathf.Sqrt(area / Mathf.PI);
this.model = model ?? new Model();
this.air_density = 1.23f;
this.force_per_v2 = 0.5f * air_density * area;
this.torque_per_v2 = force_per_v2 * diameter;
this.z_hat = new Vector3(0, 0, 1);
rb = GetComponent<Rigidbody>();
this.rb.AddForce((Vector3.forward + Vector3.up) * 1500f);
this.rb.AddTorque(Vector3.up * 500f);
}
private void FixedUpdate()
{
var computeForcesAndTorques = ComputeForcesAndTorques(this.transform.rotation.x, this.transform.rotation.z, this.rb.velocity,this.rb.angularVelocity);
var force = (Vector3)computeForcesAndTorques["Acceleration"];
var torque = (Vector3)computeForcesAndTorques["T_total"];
this.rb.AddForce(force);
this.rb.AddTorque(torque);
}
public static (float, float, float, float, float, Matrix4x4, float, Vector3) ComputeAngleOfAttack(float phi, float theta, Vector3 velocity)
{
float sp = Mathf.Sin(phi);
float cp = Mathf.Cos(phi);
float st = Mathf.Sin(theta);
float ct = Mathf.Cos(theta);
Matrix4x4 rotationMatrix = RotationMatrix(sp, cp, st, ct);
Vector3 zhat = rotationMatrix.GetColumn(2);
float vDotZhat = Vector3.Dot(velocity, zhat);
Vector3 vInPlane = velocity - zhat * vDotZhat;
float angleOfAttack = -Mathf.Atan2(vDotZhat, vInPlane.magnitude);
return (angleOfAttack, sp, cp, st, ct, rotationMatrix, vDotZhat, vInPlane);
}
public Dictionary<string, object> ComputeForcesAndTorques(float phi, float theta, Vector3 velocity, Vector3 angularVelocity)
{
Dictionary<string, object> result = new Dictionary<string, object>();
var angleOfAttackResult = ComputeAngleOfAttack(phi, theta, velocity);
float angleOfAttack = angleOfAttackResult.Item1;
float sp = angleOfAttackResult.Item2;
float cp = angleOfAttackResult.Item3;
float st = angleOfAttackResult.Item4;
float ct = angleOfAttackResult.Item5;
Matrix4x4 rotationMatrix = angleOfAttackResult.Item6;
float vDotZhat = angleOfAttackResult.Item7;
Vector3 vInPlane = angleOfAttackResult.Item8;
Vector3 zhat = rotationMatrix.GetColumn(2);
Vector3 xhat = vInPlane.normalized;
Vector3 yhat = Vector3.Cross(zhat, xhat);
float forceAmplitude = force_per_v2 * velocity.sqrMagnitude;
// Lift force
Vector3 F_lift = model.C_lift(angleOfAttack) * forceAmplitude * Vector3.Cross(velocity.normalized, yhat);
// Drag force
Vector3 F_drag = model.C_drag(angleOfAttack) * forceAmplitude * (-velocity.normalized);
// Total force
Vector3 F_total = F_lift + F_drag;
// Acceleration
Vector3 acceleration = F_total / this.rb.mass * 0.02f;
result.Add("Acceleration", acceleration);
// Torque
float torqueAmplitude = torque_per_v2 * velocity.sqrMagnitude;
Vector3 w = new Vector3(angularVelocity.x * ct, angularVelocity.y, angularVelocity.x * st + angularVelocity.z);
Vector3 T_x_lab = model.C_x(w.x, w.z) * torqueAmplitude * xhat;
Vector3 T_y_lab = model.C_y(angleOfAttack, w.y) * torqueAmplitude * yhat;
Vector3 T_z = model.C_z(w.z) * torqueAmplitude * z_hat;
// Rotate into the disc frame
Vector3 T_x = rotationMatrix * T_x_lab;
Vector3 T_y = rotationMatrix * T_y_lab;
// Total torque
Vector3 T_total = T_x + T_y + T_z;
result.Add("T_total", T_total);
return result;
}
private static Matrix4x4 RotationMatrix(float sp, float cp, float st, float ct)
{
Matrix4x4 matrix = new Matrix4x4();
matrix.SetColumn(0, new Vector4(ct, 0, st, 0));
matrix.SetColumn(1, new Vector4(sp * st, cp, -st * cp, 0));
matrix.SetColumn(2, new Vector4(-st * cp, -sp * st, cp * ct, 0));
matrix.SetColumn(3, new Vector4(0, 0, 0, 1));
return matrix;
}
}
날아가긴하는데... 뭔가 축을 잘못잡았는지 맥아리가 없다.
어떤 부분에서 문제가 발생했는지 다시 하나씩 짚어보며 찾아봐야 할 것 같다.