public override PShape3D CreateShape(GameObject root)
{
Fix64Vec3 s = CalculateSize();
if (s != Fix64Vec3.zero)
{
_currentSize = s;
Fix64Vec3 center = Fix64Vec3.zero;
Fix64Quat rotation = Fix64Quat.identity;
if (gameObject != root)
{
center = _pTransform.localPosition;
rotation = _pTransform.localRotation;
}
_shape = Parallel3D.CreatePolyhedron(convexData, s, center, rotation);
if (createUnityPhysicsCollider)
{
var collider = gameObject.AddComponent <MeshCollider>();
collider.convex = true;
}
return(_shape);
}
else
{
return(null);
}
}
protected override void UpdateShape(GameObject root)
{
Fix64 h = CalculateHeight();
Fix64 r = CalculateRadius();
Fix64Vec3 p1 = Fix64Vec3.zero;
Fix64Vec3 p2 = Fix64Vec3.zero;
CalculatePoints(h, r, ref p1, ref p2);
if (p1 == Fix64Vec3.zero || p2 == Fix64Vec3.zero)
{
Debug.LogError("Invalid Size");
return;
}
Fix64Vec3 center = Fix64Vec3.zero;
Fix64Quat rotation = Fix64Quat.identity;
if (gameObject != root)
{
center = _pTransform.localPosition;
rotation = _pTransform.localRotation;
}
point1 = p1;
point2 = p2;
radius = r;
height = h;
Parallel3D.UpdateCapsule(_shape, _fixture, p1, p2, radius, center, rotation);
}
public static Fix64Vec3 MulT(Fix64Vec3 pos, Fix64Quat rot, Fix64Vec3 point)
{
Fix64Vec3 output = Fix64Vec3.zero;
NativeParallel3D.MulT(pos, rot, point, ref output);
return(output);
}
public bool LoadSavedExport(UInt32 step)
{
if (step > _maxStep)
{
return(false);
}
if (step < _minStep)
{
return(false);
}
int index = CalculateExportIndex(step);
PBodyExport3D export = bodyExports[index];
linearVelocity = export.linearVelocity;
angularVelocity = export.angularVelocity;
position = export.position;
orientation = export.orientation;
orientation0 = export.orientation0;
awake = export.awake;
sleepTime = export.sleepTime;
Parallel3D.UpdateBodyTransformForRollback(this, position, orientation, orientation0);
Parallel3D.UpdateBodyVelocity(this, linearVelocity, angularVelocity);
Parallel3D.SetAwakeForRollback(this, awake, sleepTime);
return(true);
}
//https://stackoverflow.com/a/51586282/1368748
public static Fix64Mat4x4 FromTRS(Fix64Vec3 translation, Fix64Quat rotation, Fix64Vec3 scale)
{
Fix64 x1 = translation.x;
Fix64 y1 = translation.y;
Fix64 z1 = translation.z;
Fix64 x2 = scale.x;
Fix64 y2 = scale.y;
Fix64 z2 = scale.z;
Fix64Mat3x3 rot = new Fix64Mat3x3(rotation);
Fix64 a11 = rot.x.x;
Fix64 a21 = rot.x.y;
Fix64 a31 = rot.x.z;
Fix64 a12 = rot.y.x;
Fix64 a22 = rot.y.y;
Fix64 a32 = rot.y.z;
Fix64 a13 = rot.z.x;
Fix64 a23 = rot.z.y;
Fix64 a33 = rot.z.z;
Fix64Vec4 _x = new Fix64Vec4(x2 * a11, x2 * a21, x2 * a31, Fix64.zero);
Fix64Vec4 _y = new Fix64Vec4(y2 * a12, y2 * a22, y2 * a32, Fix64.zero);
Fix64Vec4 _z = new Fix64Vec4(z2 * a13, z2 * a23, z2 * a33, Fix64.zero);
Fix64Vec4 _w = new Fix64Vec4(x1, y1, z1, Fix64.one);
return(new Fix64Mat4x4(_x, _y, _z, _w));
}
public override PShape3D CreateShape(GameObject root)
{
Fix64Vec3 s = CalculateSize();
if (s != Fix64Vec3.zero)
{
Fix64Vec3 center = Fix64Vec3.zero;
Fix64Quat rotation = Fix64Quat.identity;
if (gameObject != root)
{
center = _pTransform.localPosition;
rotation = _pTransform.localRotation;
}
_shape = Parallel3D.CreateCube(s.x, s.y, s.z, center, rotation);
if (createUnityPhysicsCollider)
{
var collider = gameObject.AddComponent <BoxCollider>();
collider.size = (Vector3)size;
}
return(_shape);
}
else
{
return(null);
}
}
public static Fix64 LengthSqr(Fix64Quat a)
{
return(Fix64.FromRaw(NativeFixedMath.Mul64(a.RawX, a.RawX)
+ NativeFixedMath.Mul64(a.RawY, a.RawY)
+ NativeFixedMath.Mul64(a.RawZ, a.RawZ)
+ NativeFixedMath.Mul64(a.RawW, a.RawW)));
}
public static bool OverlapCube(Fix64Vec3 center, Fix64Quat rotation, Fix64 x, Fix64 y, Fix64 z, int mask, PShapeOverlapResult3D shapeOverlapResult)
{
if (!initialized)
{
Initialize();
}
int count = 0;
bool hit = NativeParallel3D.CubeOverlap(internalWorld.IntPointer, mask, center, rotation, x, y, z, _queryBodyIDs, ref count);
shapeOverlapResult.count = count;
for (int i = 0; i < count; i++)
{
UInt16 bodyID = _queryBodyIDs[i];
if (bodyDictionary.ContainsKey(bodyID))
{
shapeOverlapResult.rigidbodies[i] = bodyDictionary[bodyID].RigidBody;
}
else
{
Debug.LogError($"Rigibody not found: {bodyID}");
}
}
return(hit);
}
/// <summary>
/// Imports the floating point values from the Unity Transform Component
/// NOT deterministic
/// </summary>
public void ImportFromUnity()
{
//Debug.LogWarning("ImportFromUnity");
_localPosition = (Fix64Vec3)transform.localPosition;
_localRotation = (Fix64Quat)transform.localRotation;
_localEularAngles = (Fix64Vec3)transform.localEulerAngles;
_localScale = (Fix64Vec3)transform.localScale;
}
public static Fix64Quat FromEulerAngles(Fix64Vec3 value)
{
Fix64 yaw_y = Fix64.DegToRad(value.y);
Fix64 pitch_x = Fix64.DegToRad(value.x);
Fix64 roll_z = Fix64.DegToRad(value.z);
return(Fix64Quat.FromYawPitchRoll(yaw_y, pitch_x, roll_z));
}
public static void UpdateBodyTransForm(PBody3D body, Fix64Vec3 pos, Fix64Quat rot)
{
if (!initialized)
{
Initialize();
}
NativeParallel3D.UpdateBodyTransform(body.IntPointer, pos, rot);
}
public static Fix64Quat Inverse(Fix64Quat a)
{
long inv_norm = NativeFixedMath.Rcp64(LengthSqr(a).Raw);
return(Make(
-NativeFixedMath.Mul64(a.RawX, inv_norm),
-NativeFixedMath.Mul64(a.RawY, inv_norm),
-NativeFixedMath.Mul64(a.RawZ, inv_norm),
NativeFixedMath.Mul64(a.RawW, inv_norm)));
}
public static Fix64Quat Normalize(Fix64Quat a)
{
long inv_norm = NativeFixedMath.Rcp64(LengthFastest(a).Raw);
Fix64 x = Fix64.FromRaw(NativeFixedMath.Mul64(a.RawX, inv_norm));
Fix64 y = Fix64.FromRaw(NativeFixedMath.Mul64(a.RawY, inv_norm));
Fix64 z = Fix64.FromRaw(NativeFixedMath.Mul64(a.RawZ, inv_norm));
Fix64 w = Fix64.FromRaw(NativeFixedMath.Mul64(a.RawW, inv_norm));
return(new Fix64Quat(x, y, z, w));
}
internal static extern IntPtr CreateBody(IntPtr worldHandle,
int bodyType,
Fix64Vec3 position,
Fix64Quat orientation,
Fix64Vec3 linearDamping,
Fix64Vec3 angularDamping,
Fix64Vec3 gravityScale,
bool fixedRotationX,
bool fixedRotationY,
bool fixedRotationZ,
ref UInt16 bodyID);
public Fix64Mat3x3(Fix64Quat quat)
{
Fix64 _x = quat.x;
Fix64 _y = quat.y;
Fix64 _z = quat.z;
Fix64 _w = quat.w;
Fix64 x2 = _x + _x, y2 = _y + _y, z2 = _z + _z;
Fix64 xx = _x * x2, xy = _x * y2, xz = _x * z2;
Fix64 yy = _y * y2, yz = _y * z2, zz = _z * z2;
Fix64 wx = _w * x2, wy = _w * y2, wz = _w * z2;
x = new Fix64Vec3(Fix64.one - (yy + zz), xy + wz, xz - wy);
y = new Fix64Vec3(xy - wz, Fix64.one - (xx + zz), yz + wx);
z = new Fix64Vec3(xz + wy, yz - wx, Fix64.one - (xx + yy));
}
protected override void UpdateShape(GameObject root)
{
Fix64Vec3 s = CalculateSize();
if (s != Fix64Vec3.zero)
{
Fix64Vec3 center = Fix64Vec3.zero;
Fix64Quat rotation = Fix64Quat.identity;
if (gameObject != root)
{
center = _pTransform.localPosition;
rotation = _pTransform.localRotation;
}
Parallel3D.UpdateCube(_shape, _fixture, s.x, s.y, s.z, center, rotation);
}
}
public static Fix64Quat LookRotation(Fix64Vec3 dir, Fix64Vec3 up)
{ // From: https://answers.unity.com/questions/819699/calculate-quaternionlookrotation-manually.html
if (dir == Fix64Vec3.zero)
{
return(identity);
}
if (up != dir)
{
Fix64Vec3 v = dir + up * -Fix64Vec3.Dot(up, dir);
Fix64Quat q = FromTwoVectors(Fix64Vec3.forward, v);
return(FromTwoVectors(v, dir) * q);
}
else
{
return(FromTwoVectors(Fix64Vec3.forward, dir));
}
}
public static Fix64Quat Slerp(Fix64Quat q1, Fix64Quat q2, Fix64 t)
{
Fix64 epsilon = Fix64.FromDivision(1, 1000000);
Fix64 cos_omega = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
bool flip = false;
if (cos_omega < Fix64.zero)
{
flip = true;
cos_omega = -cos_omega;
}
Fix64 s1, s2;
if (cos_omega > (Fix64.one - epsilon))
{
// Too close, do straight linear interpolation.
s1 = Fix64.one - t;
s2 = (flip) ? -t : t;
}
else
{
Fix64 omega = Fix64.FromRaw(NativeFixedMath.Acos64(cos_omega.Raw));
Fix64 inv_sin_omega = Fix64.one / Fix64.FromRaw(NativeFixedMath.Sin64(omega.Raw));
Fix64 v1 = (Fix64.one - t) * omega;
Fix64 v2 = t * omega;
s1 = Fix64.FromRaw(NativeFixedMath.Sin64(v1.Raw)) * inv_sin_omega;
s2 = Fix64.FromRaw(NativeFixedMath.Sin64(v2.Raw)) * inv_sin_omega;
s2 = (flip) ? -s2 : s2;
}
return(new Fix64Quat(
s1 * q1.x + s2 * q2.x,
s1 * q1.y + s2 * q2.y,
s1 * q1.z + s2 * q2.z,
s1 * q1.w + s2 * q2.w));
}
//3D body
public static PBody3D AddBody(
int bodyType,
Fix64Vec3 position,
Fix64Quat orientation,
Fix64Vec3 linearDamping,
Fix64Vec3 angularDamping,
Fix64Vec3 gravityScale,
bool fixedRotationX,
bool fixedRotationY,
bool fixedRotationZ,
IParallelRigidbody3D rigidBody)
{
if (!initialized)
{
Initialize();
}
UInt16 bodyID = 0;
IntPtr m_NativeObject = NativeParallel3D.CreateBody(
internalWorld.IntPointer,
bodyType,
position,
orientation,
linearDamping,
angularDamping,
gravityScale,
fixedRotationX,
fixedRotationY,
fixedRotationZ,
ref bodyID);
PBody3D body = new PBody3D(m_NativeObject, bodyID, rigidBody as ParallelRigidbody3D, bodyExportSize);
rigidBodies.Add(rigidBody);
bodyDictionary[bodyID] = body;
ReadNativeBody(body);
return(body);
}
public override PShape3D CreateShape(GameObject root)
{
Fix64 h = CalculateHeight();
Fix64 r = CalculateRadius();
Fix64Vec3 p1 = Fix64Vec3.zero;
Fix64Vec3 p2 = Fix64Vec3.zero;
CalculatePoints(h, r, ref p1, ref p2);
if (p1 == Fix64Vec3.zero || p2 == Fix64Vec3.zero)
{
Debug.LogError("Invalid Size");
return(null);
}
else
{
Fix64Vec3 center = Fix64Vec3.zero;
Fix64Quat rotation = Fix64Quat.identity;
if (gameObject != root)
{
center = _pTransform.localPosition;
rotation = _pTransform.localRotation;
}
point1 = p1;
point2 = p2;
radius = r;
height = h;
_shape = Parallel3D.CreateCapsule(p1, p2, radius, center, rotation);
if (createUnityPhysicsCollider)
{
var collider = gameObject.AddComponent <CapsuleCollider>();
collider.height = (float)h;
collider.radius = (float)radius;
}
return(_shape);
}
}
internal static extern IntPtr CreateConvex(Fix64Vec3[] verts, UInt32 vertsCount, ParallelEdge[] edges, UInt32 edgesCount, ParallelFace[] faces, UInt32 faceCount, ParallelPlane[] planes, Fix64Vec3 scale, Fix64Vec3 center, Fix64Quat rotation);
internal static extern IntPtr CreateCapsule(Fix64Vec3 v1, Fix64Vec3 v2, Fix64 radius, Fix64Vec3 center, Fix64Quat rotation);
internal static extern void UpdateCube(IntPtr shapeHandle, IntPtr fixtureHandle, Fix64 x, Fix64 y, Fix64 z, Fix64Vec3 center, Fix64Quat rotation);
internal static extern bool CubeOverlap(IntPtr worldHandle, int mask, Fix64Vec3 center, Fix64Quat rot, Fix64 x, Fix64 y, Fix64 z, UInt16[] bodyIDs, ref int count);
internal static extern void UpdateCapsule(IntPtr shapeHandle, IntPtr fixtureHandle, Fix64Vec3 v1, Fix64Vec3 v2, Fix64 radius, Fix64Vec3 center, Fix64Quat rotation);
internal static extern Fix64Vec3 MulT(Fix64Vec3 pos, Fix64Quat rot, Fix64Vec3 point, ref Fix64Vec3 output);
internal void _internal_WriteTranform(Fix64Vec3 position, Fix64Quat rotation)
{
_localPosition = position;
_internalLocalRotation = rotation;
ExportToUnity();
}
public void RotateInLocalSpace(Fix64Vec3 eulers)
{
//Rotation = Fix64Quat.FromEulerAngles(eulers) * Rotation;
localRotation = localRotation * Fix64Quat.FromEulerAngles(eulers);
}
//rotate in degree
public void RotateInWorldSpace(Fix64Vec3 eulers)
{
//Rotation = Fix64Quat.FromEulerAngles(eulers) * Rotation;
rotation = rotation * Fix64Quat.FromEulerAngles(eulers);
}
internal static extern void UpdateBodyTransformForRollback(IntPtr bodyHandle, Fix64Vec3 position, Fix64Quat orientation, Fix64Quat orientation0);