407 lines
13 KiB
C#
407 lines
13 KiB
C#
|
using System;
|
||
|
|
using System.Collections.Generic;
|
||
|
|
using System.Linq;
|
||
|
|
using System.Numerics;
|
||
|
|
using System.Text;
|
||
|
|
using System.Threading.Tasks;
|
||
|
|
using MagicPhysX; // for enable Extension Methods.
|
||
|
|
using MagicPhysX.Toolkit;
|
||
|
|
using static System.Formats.Asn1.AsnWriter;
|
||
|
|
using static MagicPhysX.NativeMethods; // recommend to use C API.
|
||
|
|
|
||
|
|
namespace Physx
|
||
|
|
{
|
||
|
|
|
||
|
|
using PxI64 = Int64;
|
||
|
|
using PxU64 = UInt64;
|
||
|
|
using PxI32 = Int32;
|
||
|
|
using PxU32 = UInt32;
|
||
|
|
using PxI16 = Int16;
|
||
|
|
using PxU16 = UInt16;
|
||
|
|
using PxI8 = Byte;
|
||
|
|
using PxU8 = SByte;
|
||
|
|
//using PxF32 = System.Runtime.InteropServices.NFloat;
|
||
|
|
//using PxF64 =double;
|
||
|
|
//using PxReal = float;
|
||
|
|
|
||
|
|
|
||
|
|
public unsafe class PhysxCommon : IDisposable
|
||
|
|
{
|
||
|
|
|
||
|
|
public const float PxPi = 3.141592653589793f;
|
||
|
|
public const float PxHalfPi = 1.57079632679489661923f;
|
||
|
|
public const float PxTwoPi = 6.28318530717958647692f;
|
||
|
|
public const float PxInvPi = 0.31830988618379067154f;
|
||
|
|
public const float PxInvTwoPi = 0.15915494309189533577f;
|
||
|
|
public const float PxPiDivTwo = 1.57079632679489661923f;
|
||
|
|
public const float PxPiDivFour = 0.78539816339744830962f;
|
||
|
|
|
||
|
|
|
||
|
|
public static PxFoundation* _Foundation;
|
||
|
|
public static PxPhysics* _Physics;
|
||
|
|
public static PxMaterial* _Material;
|
||
|
|
public static PxDefaultCpuDispatcher* _Dispatcher;
|
||
|
|
public static PxPvd* _Pvd;
|
||
|
|
|
||
|
|
public static PxAllocatorCallback _Allocator;
|
||
|
|
public static PxDefaultErrorCallback _ErrorCallback;
|
||
|
|
|
||
|
|
bool disposedValue;
|
||
|
|
|
||
|
|
|
||
|
|
public PhysxCommon() { }
|
||
|
|
public static PhysxCommon Instance { get { return instance; } }
|
||
|
|
private static readonly PhysxCommon instance = new PhysxCommon();
|
||
|
|
|
||
|
|
|
||
|
|
//系统自动调用
|
||
|
|
~PhysxCommon()
|
||
|
|
{
|
||
|
|
Dispose(disposing: false);
|
||
|
|
}
|
||
|
|
|
||
|
|
private void Dispose(bool disposing)
|
||
|
|
{
|
||
|
|
if (!disposedValue)
|
||
|
|
{
|
||
|
|
if (disposing)
|
||
|
|
{
|
||
|
|
// cleanup managed code
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
// cleanup unmanaged resource
|
||
|
|
disposedValue = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//手动调用
|
||
|
|
public void Dispose()
|
||
|
|
{
|
||
|
|
Dispose(disposing: true);
|
||
|
|
GC.SuppressFinalize(this);
|
||
|
|
}
|
||
|
|
|
||
|
|
//physx_common::~physx_common() { Destory(); }
|
||
|
|
|
||
|
|
public bool InitPhyxsSDK()
|
||
|
|
{
|
||
|
|
|
||
|
|
// create physics
|
||
|
|
uint PX_PHYSICS_VERSION_MAJOR = 5;
|
||
|
|
uint PX_PHYSICS_VERSION_MINOR = 1;
|
||
|
|
uint PX_PHYSICS_VERSION_BUGFIX = 3;
|
||
|
|
uint versionNumber = (PX_PHYSICS_VERSION_MAJOR << 24) + (PX_PHYSICS_VERSION_MINOR << 16) + (PX_PHYSICS_VERSION_BUGFIX << 8);
|
||
|
|
if (_Foundation == null)
|
||
|
|
{
|
||
|
|
fixed (PxAllocatorCallback* allocPtr = &_Allocator)
|
||
|
|
{
|
||
|
|
fixed (PxDefaultErrorCallback* errorPtr = &_ErrorCallback)
|
||
|
|
{
|
||
|
|
//_Foundation = phys_PxCreateFoundation(versionNumber, allocPtr,
|
||
|
|
//errorPtr);
|
||
|
|
// _Foundation =
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
if (_Foundation == null)
|
||
|
|
{
|
||
|
|
//LogMgr::GetInstance()->Error(
|
||
|
|
// "physx_common::InitPhyxsSDK() error:px_foundation_ is null");
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//if (!px_pvd_)
|
||
|
|
//{
|
||
|
|
// px_pvd_ = PxCreatePvd(*px_foundation_);
|
||
|
|
//}
|
||
|
|
//if (px_pvd_ == nullptr) return false;
|
||
|
|
|
||
|
|
|
||
|
|
if (_Physics == null)
|
||
|
|
{
|
||
|
|
|
||
|
|
var tolerancesScale = new PxTolerancesScale { length = 1, speed = 10 };
|
||
|
|
_Physics = phys_PxCreatePhysics(versionNumber, _Foundation, &tolerancesScale, true, _Pvd, null);
|
||
|
|
|
||
|
|
}
|
||
|
|
if (_Physics == null)
|
||
|
|
{
|
||
|
|
//LogMgr::GetInstance()->Error(
|
||
|
|
// "physx_common::InitPhyxsSDK() error: px_physics_ is null");
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_Dispatcher == null)
|
||
|
|
//_Dispatcher = phys_PxDefaultCpuDispatcherCreate(0);
|
||
|
|
phys_PxDefaultCpuDispatcherCreate(1, null, PxDefaultCpuDispatcherWaitForWorkMode.WaitForWork, 0);
|
||
|
|
if (_Material == null)
|
||
|
|
// _Material = _Physics->createMaterial(0.0f, 0.0f, 0.0f);
|
||
|
|
_Material = PxPhysics_createMaterial_mut(_Physics, 0.0f, 0.0f, 0.0f);
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
//public void Destory()
|
||
|
|
//{
|
||
|
|
//if (instance_ != nullptr)
|
||
|
|
//{
|
||
|
|
// ReleaseSDK();
|
||
|
|
// delete instance_;
|
||
|
|
// instance_ = nullptr;
|
||
|
|
//}
|
||
|
|
//}
|
||
|
|
|
||
|
|
|
||
|
|
bool IsConnectedPvd()
|
||
|
|
{
|
||
|
|
// return _Pvd !=null ? _Pvd->PxPvd_isConnected_mut() : false;
|
||
|
|
return _Pvd != null ? PxPvd_isConnected_mut(_Pvd, true) : false;
|
||
|
|
}
|
||
|
|
|
||
|
|
public void ConnectPvdServer()
|
||
|
|
{
|
||
|
|
if (IsConnectedPvd() == false)
|
||
|
|
return;
|
||
|
|
if (_Pvd == null)
|
||
|
|
return;
|
||
|
|
|
||
|
|
//PxPvdTransport* transport = PxDefaultPvdSocketTransportCreate(g_pGameWorld->m_Settings.Ip_Pvd, 5425, 10);
|
||
|
|
byte hostip = 10;
|
||
|
|
PxPvdTransport* transport = phys_PxDefaultPvdSocketTransportCreate(&hostip, 5425, 10);
|
||
|
|
|
||
|
|
if (transport == null)
|
||
|
|
return;
|
||
|
|
|
||
|
|
bool is_connected = _Pvd->ConnectMut(transport, PxPvdInstrumentationFlags.All);
|
||
|
|
if (!is_connected)
|
||
|
|
{
|
||
|
|
//LogMgr::GetInstance()->Error(
|
||
|
|
// "physx_common::InitPhyxsSDK() error:pvd connect error.");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public void DisconnectPvdServer()
|
||
|
|
{
|
||
|
|
if (!IsConnectedPvd())
|
||
|
|
return;
|
||
|
|
if (_Pvd == null) return;
|
||
|
|
|
||
|
|
_Pvd->DisconnectMut();
|
||
|
|
}
|
||
|
|
|
||
|
|
public void ReleaseSDK()
|
||
|
|
{
|
||
|
|
if (_Physics != null)
|
||
|
|
{
|
||
|
|
_Dispatcher->ReleaseMut();
|
||
|
|
_Physics->ReleaseMut();
|
||
|
|
if (_Pvd != null)
|
||
|
|
{
|
||
|
|
PxPvdTransport* transport = _Pvd->GetTransportMut();
|
||
|
|
if (transport != null)
|
||
|
|
{
|
||
|
|
transport->ReleaseMut();
|
||
|
|
transport = null;
|
||
|
|
}
|
||
|
|
_Pvd->ReleaseMut();
|
||
|
|
_Pvd = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
_Foundation->ReleaseMut();
|
||
|
|
_Dispatcher = null;
|
||
|
|
_Physics = null;
|
||
|
|
_Foundation = null;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public PxPhysics* GetPhysics() { return _Physics; }
|
||
|
|
public PxDefaultCpuDispatcher* GetCpuDispatcher() { return _Dispatcher; }
|
||
|
|
public PxMaterial* GetDefaultMaterial() { return _Material; }
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////////////////////////////
|
||
|
|
// maths tools
|
||
|
|
/////////////////////////////////////////////////////////////////////////
|
||
|
|
void SetEulerYXZ(ref float yaw, ref float pitch,
|
||
|
|
ref float roll, ref PxQuat quat)
|
||
|
|
{
|
||
|
|
float halfYaw = yaw * 0.5f;
|
||
|
|
float halfPitch = pitch * 0.5f;
|
||
|
|
float halfRoll = roll * 0.5f;
|
||
|
|
float cosYaw = MathF.Cos(halfYaw);
|
||
|
|
float sinYaw = MathF.Sin(halfYaw);
|
||
|
|
float cosPitch = MathF.Cos(halfPitch);
|
||
|
|
float sinPitch = MathF.Sin(halfPitch);
|
||
|
|
float cosRoll = MathF.Cos(halfRoll);
|
||
|
|
float sinRoll = MathF.Sin(halfRoll);
|
||
|
|
|
||
|
|
//效率更高
|
||
|
|
quat.x = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
|
||
|
|
quat.y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
|
||
|
|
quat.z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
|
||
|
|
quat.w = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
|
||
|
|
}
|
||
|
|
|
||
|
|
//PxVec3 GetQuatRotate(KVector3D& dir, KVector3D& velocity)
|
||
|
|
//{
|
||
|
|
// PxQuat px_qua;
|
||
|
|
// SetEulerYXZ((float)((PI_F * dir.nY) / (1000 * 180)), (float)((PI_F * dir.nX) / (1000 * 180)),
|
||
|
|
// (float)((PI_F * dir.nZ) / (1000 * 180)), px_qua);
|
||
|
|
|
||
|
|
// return px_qua.rotate(PxVec3((float)velocity.nX, (float)velocity.nY,
|
||
|
|
// (float)velocity.nZ));
|
||
|
|
//}
|
||
|
|
|
||
|
|
PxVec3 QuatToEuler(PxQuat* q)
|
||
|
|
{
|
||
|
|
//PxVec3 dir = -PxMat33(q)[2];
|
||
|
|
PxVec3 dir = PxVec3_new_3(-q->Mat33New6().column2.x, -q->Mat33New6().column2.y, -q->Mat33New6().column2.z);
|
||
|
|
float r = MathF.Sqrt(dir.x * dir.x + dir.z * dir.z);
|
||
|
|
PxVec3 rot = PxVec3_new_3(0.0f, PxHalfPi, 0.0f);
|
||
|
|
if (r != 0.0f)
|
||
|
|
{
|
||
|
|
rot.x = -MathF.Atan(dir.y / r);
|
||
|
|
rot.y = MathF.Asin(dir.x / r);
|
||
|
|
if (dir.z > 0.0f) rot.y = PxPi - rot.y;
|
||
|
|
}
|
||
|
|
return rot;
|
||
|
|
}
|
||
|
|
|
||
|
|
PxVec3 GetEuler(PxVec3 forward)
|
||
|
|
{
|
||
|
|
PxVec3 euler = PxVec3_new_3(0, 0, 0);
|
||
|
|
PxVec3 tmp_forward = forward;
|
||
|
|
tmp_forward.y = 0;
|
||
|
|
//tmp_forward = BaseBehavior::SafeNormalized(tmp_forward);
|
||
|
|
|
||
|
|
// xz平面
|
||
|
|
//float angle = BaseBehavior::PxVec3Angle(PxVec3(0, 0, 1), tmp_forward) /
|
||
|
|
// PxPi * 180; // 角度值
|
||
|
|
//TODO:需要重写
|
||
|
|
float angle = 1.0f;
|
||
|
|
if (tmp_forward.x >= 0)
|
||
|
|
{
|
||
|
|
euler.y = angle;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
euler.y = -angle;
|
||
|
|
}
|
||
|
|
|
||
|
|
// pitch角度
|
||
|
|
tmp_forward = forward;
|
||
|
|
tmp_forward.z = MathF.Sqrt(tmp_forward.x * tmp_forward.x + tmp_forward.z * tmp_forward.z);
|
||
|
|
tmp_forward.x = 0;
|
||
|
|
//tmp_forward = BaseBehavior::SafeNormalized(tmp_forward);
|
||
|
|
//angle = BaseBehavior::PxVec3Angle(PxVec3(0, 0, 1), tmp_forward) /
|
||
|
|
// PxPi * 180; // 角度值
|
||
|
|
if (tmp_forward.y >= 0)
|
||
|
|
{
|
||
|
|
euler.x = -angle;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
euler.x = angle;
|
||
|
|
}
|
||
|
|
return euler;
|
||
|
|
}
|
||
|
|
|
||
|
|
float GetAngle(PxVec3* v0, PxVec3* v1)
|
||
|
|
{
|
||
|
|
float m = v0->MagnitudeSquared() * v1->MagnitudeSquared();
|
||
|
|
float s = MathF.Sqrt(m);
|
||
|
|
|
||
|
|
if (s < (float)(1e-20f))
|
||
|
|
{
|
||
|
|
return 0.0f;
|
||
|
|
}
|
||
|
|
|
||
|
|
s = 1.0f / s;
|
||
|
|
return MathF.Acos(v0->Dot(v1) * s);
|
||
|
|
}
|
||
|
|
|
||
|
|
void QuatToMatrix(float* quat, float* matrix)
|
||
|
|
{
|
||
|
|
float xx = quat[0] * quat[0];
|
||
|
|
float yy = quat[1] * quat[1];
|
||
|
|
float zz = quat[2] * quat[2];
|
||
|
|
float xy = quat[0] * quat[1];
|
||
|
|
float xz = quat[0] * quat[2];
|
||
|
|
float yz = quat[1] * quat[2];
|
||
|
|
float wx = quat[3] * quat[0];
|
||
|
|
float wy = quat[3] * quat[1];
|
||
|
|
float wz = quat[3] * quat[2];
|
||
|
|
|
||
|
|
matrix[0 * 4 + 0] = 1 - 2 * (yy + zz);
|
||
|
|
matrix[1 * 4 + 0] = 2 * (xy - wz);
|
||
|
|
matrix[2 * 4 + 0] = 2 * (xz + wy);
|
||
|
|
|
||
|
|
matrix[0 * 4 + 1] = 2 * (xy + wz);
|
||
|
|
matrix[1 * 4 + 1] = 1 - 2 * (xx + zz);
|
||
|
|
matrix[2 * 4 + 1] = 2 * (yz - wx);
|
||
|
|
|
||
|
|
matrix[0 * 4 + 2] = 2 * (xz - wy);
|
||
|
|
matrix[1 * 4 + 2] = 2 * (yz + wx);
|
||
|
|
matrix[2 * 4 + 2] = 1 - 2 * (xx + yy);
|
||
|
|
|
||
|
|
matrix[3 * 4 + 0] = matrix[3 * 4 + 1] = matrix[3 * 4 + 2] =
|
||
|
|
(float)0.0f;
|
||
|
|
matrix[0 * 4 + 3] = matrix[1 * 4 + 3] = matrix[2 * 4 + 3] =
|
||
|
|
(float)0.0f;
|
||
|
|
matrix[3 * 4 + 3] = (float)1.0f;
|
||
|
|
}
|
||
|
|
|
||
|
|
PxVec3 ToEulerAngle(float* quat)
|
||
|
|
{
|
||
|
|
// float matrix[16];
|
||
|
|
|
||
|
|
float* matrix = stackalloc float[16];
|
||
|
|
QuatToMatrix(quat, matrix);
|
||
|
|
|
||
|
|
float m11 = matrix[0], m12 = matrix[4], m13 = matrix[8];
|
||
|
|
float m21 = matrix[1], m22 = matrix[5], m23 = matrix[9];
|
||
|
|
float m31 = matrix[2], m32 = matrix[6], m33 = matrix[10];
|
||
|
|
|
||
|
|
if (m23 < -1)
|
||
|
|
{
|
||
|
|
m23 = -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (m23 > 1)
|
||
|
|
{
|
||
|
|
m23 = 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
float _x, _y, _z;
|
||
|
|
_x = MathF.Asin(-m23);
|
||
|
|
|
||
|
|
if (MathF.Abs(m23) < 0.99999)
|
||
|
|
{
|
||
|
|
_y = MathF.Atan2(m13, m33);
|
||
|
|
_z = MathF.Atan2(m21, m22);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
_y = MathF.Atan2(-m31, m11);
|
||
|
|
_z = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
return PxVec3_new_3(_x, _y, _z);
|
||
|
|
}
|
||
|
|
|
||
|
|
public double Distance2(PxVec3* a, PxVec3* b)
|
||
|
|
{
|
||
|
|
PxVec3 tmp = a->Multiply(b);
|
||
|
|
return tmp.Dot(&tmp);
|
||
|
|
}
|
||
|
|
|
||
|
|
public double Distane(PxVec3* a, ref PxVec3* b)
|
||
|
|
{
|
||
|
|
return Math.Sqrt(Distance2(a, b));
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|