public override PShape3D CreateShape(GameObject root)
{
Fix64 r = CalculateRadius();
if (r > Fix64.zero)
{
Fix64Vec3 center = Fix64Vec3.zero;
if (gameObject != root)
{
center = _pTransform.localPosition;
}
_shape = Parallel3D.CreateSphere(r, center);
if (createUnityPhysicsCollider)
{
var collider = gameObject.AddComponent <SphereCollider>();
collider.radius = (float)radius;
}
return(_shape);
}
else
{
Debug.LogError("Invalid Size");
return(null);
}
}
void CalculatePoints(Fix64 h, Fix64 r, ref Fix64Vec3 point1, ref Fix64Vec3 point2)
{
point1 = Fix64Vec3.zero;
point2 = Fix64Vec3.zero;
Fix64 pointDistance = h - Fix64.FromDivision(2, 1) * r;
if (pointDistance <= Fix64.zero)
{
Debug.LogError("Invalid size");
return;
}
if (Direction == ParallelCapsuleDirection3D.XAxis)
{
point1 = new Fix64Vec3(Fix64.one, Fix64.zero, Fix64.zero);
point2 = new Fix64Vec3(-Fix64.one, Fix64.zero, Fix64.zero);
}
else if (Direction == ParallelCapsuleDirection3D.YAxis)
{
point1 = new Fix64Vec3(Fix64.zero, Fix64.one, Fix64.zero);
point2 = new Fix64Vec3(Fix64.zero, -Fix64.one, Fix64.zero);
}
else
{
point1 = new Fix64Vec3(Fix64.zero, Fix64.zero, Fix64.one);
point2 = new Fix64Vec3(Fix64.zero, Fix64.zero, -Fix64.one);
}
point1 = point1 * (pointDistance / Fix64.FromDivision(2, 1));
point2 = point2 * (pointDistance / Fix64.FromDivision(2, 1));
}
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);
}
}
void BuildConvexData()
{
//stan hull
Vector3[] vIn1 = new Vector3[vertsCount];
for (int i = 0; i < vertsCount; i++)
{
vIn1[i] = verts[i];
}
ParallelQHullData2 qhullData2 = Parallel3D.ConvextHull3D2(vIn1, (UInt32)vertsCount, (int)_limit);
convexData2 = qhullData2;
ParallelIntTriangle[] t = new ParallelIntTriangle[convexData2.triCount];
Array.Copy(convexData2.tris, 0, t, 0, convexData2.triCount);
convexData2.tris = t;
//new convex hull
Fix64Vec3[] vIn = new Fix64Vec3[_limit];
for (int i = 0; i < _limit; i++)
{
vIn[i] = (Fix64Vec3)convexData2.vertices[i];
}
float rad = angle * Mathf.Deg2Rad;
ParallelQHullData qhullData = Parallel3D.ConvextHull3D(vIn, (UInt32)_limit, _simplified, (Fix64)rad);
convexData = qhullData;
Fix64Vec3[] v = new Fix64Vec3[convexData.vertexCount];
Array.Copy(convexData.vertices, 0, v, 0, convexData.vertexCount);
convexData.vertices = v;
string output = "";
output += $"b3Vec3 verts[{convexData.vertexCount}] = {{}};\n";
//Debug.Log($"b3Vec3 verts[{convexData.vertexCount}] = {{}};");
for (int i = 0; i < convexData.vertexCount; i++)
{
Vector3 vec3 = (Vector3)convexData.vertices[i];
output += $"b3Vec3({vec3.x}, {vec3.y}, {vec3.z}),\n";
//Debug.Log($"verts[{i}] = b3Vec3({vec3.x}, {vec3.y}, {vec3.z});");
}
//Debug.Log(output);
ParallelEdge[] e = new ParallelEdge[convexData.edgeCount];
Array.Copy(convexData.edges, 0, e, 0, convexData.edgeCount);
convexData.edges = e;
ParallelFace[] f = new ParallelFace[convexData.faceCount];
Array.Copy(convexData.faces, 0, f, 0, convexData.faceCount);
convexData.faces = f;
ParallelPlane[] p = new ParallelPlane[convexData.faceCount];
Array.Copy(convexData.planes, 0, p, 0, convexData.faceCount);
convexData.planes = p;
return;
}
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 Fix64Vec3 TransformDirectionUnscaled(Fix64Vec3 direction)
{
Fix64Mat4x4 m = localToWorldMatrixUnscaled;
Fix64Vec3 p = m.MultiplyVector(direction);
return(p);
}
//Transforms position from world space to local space.
public Fix64Vec3 InverseTransformPoint(Fix64Vec3 position)
{
Fix64Mat4x4 m = worldToLocalMatrix;
Fix64Vec3 p = m.MultiplyPoint3x4(position);
return(p);
}
public static ParallelQHullData ConvextHull3D(Fix64Vec3[] verts, UInt32 count, bool simplify, Fix64 rad)
{
if (!initialized)
{
Initialize();
}
UInt32 outCount = 1024 * 10;
Fix64Vec3[] vertsOut = new Fix64Vec3[outCount];
ParallelEdge[] edgesOut = new ParallelEdge[outCount];
ParallelFace[] facesOut = new ParallelFace[outCount];
ParallelPlane[] planesOut = new ParallelPlane[outCount];
UInt32 vertsOutCount = outCount;
UInt32 edgesOutCount = outCount;
UInt32 facesOutCount = outCount;
NativeParallel3D.ConvexHull3D(verts, count, vertsOut, ref vertsOutCount, edgesOut, ref edgesOutCount, facesOut, ref facesOutCount, planesOut, simplify, rad);
ParallelQHullData parallelQHullData = new ParallelQHullData();
parallelQHullData.vertexCount = vertsOutCount;
parallelQHullData.edgeCount = edgesOutCount;
parallelQHullData.faceCount = facesOutCount;
parallelQHullData.vertices = vertsOut;
parallelQHullData.edges = edgesOut;
parallelQHullData.faces = facesOut;
parallelQHullData.planes = planesOut;
return(parallelQHullData);
}
public Fix64Vec3 TransformPointUnscaled(Fix64Vec3 position)
{
Fix64Mat4x4 m = localToWorldMatrixUnscaled;
Fix64Vec3 p = m.MultiplyPoint3x4(position);
return(p);
}
public static Fix64Quat FromTwoVectors(Fix64Vec3 a, Fix64Vec3 b)
{ // From: http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
Fix64 epsilon = Fix64.FromDivision(1, 1000000);
Fix64 ab = Fix64Vec3.LengthSqr(a) * Fix64Vec3.LengthSqr(b);
Fix64 norm_a_norm_b = Fix64.FromRaw(NativeFixedMath.Sqrt64(ab.Raw));
Fix64 real_part = norm_a_norm_b + Fix64Vec3.Dot(a, b);
Fix64Vec3 v;
if (real_part < (epsilon * norm_a_norm_b))
{
/* If u and v are exactly opposite, rotate 180 degrees
* around an arbitrary orthogonal axis. Axis normalization
* can happen later, when we normalize the quaternion. */
real_part = Fix64.zero;
bool cond = NativeFixedMath.Abs64(a.x.Raw) > NativeFixedMath.Abs64(a.z.Raw);
v = cond ? new Fix64Vec3(-a.y, a.x, Fix64.zero)
: new Fix64Vec3(Fix64.zero, -a.z, a.y);
}
else
{
/* Otherwise, build quaternion the standard way. */
v = Fix64Vec3.Cross(a, b);
}
return(Normalize(new Fix64Quat(v.x, v.y, v.z, real_part)));
}
//Transforms direction from world space to local space.
public Fix64Vec3 InverseTransfromDirction(Fix64Vec3 direction)
{
Fix64Mat4x4 m = worldToLocalMatrix;
Fix64Vec3 p = m.MultiplyVector(direction);
return(p);
}
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 Fix64Vec3 MulT(Fix64Vec3 pos, Fix64Quat rot, Fix64Vec3 point)
{
Fix64Vec3 output = Fix64Vec3.zero;
NativeParallel3D.MulT(pos, rot, point, ref output);
return(output);
}
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);
}
void OnDrawGizmosSelected()
{
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;
}
Gizmos.color = DebugSettings.ColliderOutlineColor;
Vector3 point1 = PMath.TransformPointUnscaled(transform, (Vector3)p1);
Vector3 point2 = PMath.TransformPointUnscaled(transform, (Vector3)p2);
Vector3 origin = (point1 - point2) / 2 + point1;
Gizmos.matrix = Matrix4x4.TRS(origin, Quaternion.identity, new Vector3((float)r, (float)r, (float)r));
DebugDraw.DrawHemispheresOfCapsule(point1, point2, (float)r);
Gizmos.matrix = Matrix4x4.identity;
DebugDraw.DrawLineConnectingHS(point1, point2, (float)r);
Gizmos.matrix = Matrix4x4.identity;
}
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);
}
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));
}
/// <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;
}
internal static extern void UpdateBodyProperties(IntPtr bodyHandle,
int bodyType,
Fix64Vec3 linearDamping,
Fix64Vec3 angularDamping,
Fix64Vec3 gravityScale,
bool fixedRotationX,
bool fixedRotationY,
bool fixedRotationZ);
//Angle in degrees between two normalized vectors
public static Fix64 Anlge(Fix64Vec3 a, Fix64Vec3 b)
{
Fix64 dot = Fix64Vec3.Dot(a.normalized, b.normalized);
Fix64 clamped = Fix64Math.Clamp(dot, -Fix64.one, Fix64.one);
Fix64 rad = Fix64.FromRaw(NativeFixedMath.Acos64(clamped.Raw));
return(rad * Fix64.RadToDegree);
}
public void ReadNative()
{
_body3D.ReadNative();
_tempAngularVelocity = _body3D.angularVelocity;
_tempLinearVelocity = _body3D.linearVelocity;
_awake = _body3D.IsAwake;
pTransform._internal_WriteTranform(_body3D.position, _body3D.orientation);
}
//Cross product of two vectors
public static Fix64Vec3 Cross(Fix64Vec3 a, Fix64Vec3 b)
{
Fix64 x = a.y * b.z - a.z * b.y;
Fix64 y = a.z * b.x - a.x * b.z;
Fix64 z = a.x * b.y - a.y * b.x;
return(new Fix64Vec3(x, y, z));
}
public static void UpdateBodyVelocity(PBody3D body, Fix64Vec3 linearVelocity, Fix64Vec3 angularVelocity)
{
if (!initialized)
{
Initialize();
}
NativeParallel3D.UpdateBodyVelocity(body.IntPointer, linearVelocity, angularVelocity);
}
internal static extern bool RayCast(
Fix64Vec3 point1,
Fix64Vec3 point2,
int mask,
ref Fix64Vec3 point,
ref Fix64Vec3 normal,
ref Fix64 fraction,
ref UInt16 bodyID,
IntPtr worldHandle);
public static void UpdateMassData(PBody3D body, Fix64 mass, Fix64Vec3 centerOfMass)
{
if (!initialized)
{
Initialize();
}
NativeParallel3D.UpdateMassData(body.IntPointer, mass, centerOfMass);
}
public static void ApplyForceToCenter(PBody3D body, Fix64Vec3 force)
{
if (!initialized)
{
Initialize();
}
NativeParallel3D.ApplyForceToCenter(body.IntPointer, force);
}
public static void ApplyTorque(PBody3D body, Fix64Vec3 torque)
{
if (!initialized)
{
Initialize();
}
NativeParallel3D.ApplyTorque(body.IntPointer, torque);
}
public static void ApplyAngularImpulse(PBody3D body, Fix64Vec3 impulse)
{
if (!initialized)
{
Initialize();
}
NativeParallel3D.ApplyAngularImpulse(body.IntPointer, impulse);
}
void OnDrawGizmosSelected()
{
Fix64Vec3 s = CalculateSize();
Gizmos.color = DebugSettings.ColliderOutlineColor;
Gizmos.matrix = Matrix4x4.TRS(transform.position, transform.rotation, Vector3.one);
Gizmos.DrawWireCube(Vector3.zero, (Vector3)s);
Gizmos.matrix = Matrix4x4.identity;
}
