internal static CapsuleGeometry BakeToBodySpace(
this CapsuleGeometry capsule, float4x4 localToWorld, float4x4 shapeToWorld,
out float3 center, out float height, ref EulerAngles orientation
)
{
using (var geometry = new NativeArray <CapsuleGeometry>(1, Allocator.TempJob)
{
[0] = capsule
})
using (var outCenter = new NativeArray <float3>(1, Allocator.TempJob))
using (var outHeight = new NativeArray <float>(1, Allocator.TempJob))
using (var outOrientation = new NativeArray <EulerAngles>(1, Allocator.TempJob)
{
[0] = orientation
})
{
var job = new BakeCapsuleJob
{
Capsule = geometry,
Center = outCenter,
Height = outHeight,
Orientation = outOrientation,
localToWorld = localToWorld,
shapeToWorld = shapeToWorld
};
job.Run();
center = outCenter[0];
height = outHeight[0];
orientation = outOrientation[0];
return(geometry[0]);
}
}
internal static CapsuleGeometry GetBakedCapsuleProperties(
this PhysicsShapeAuthoring shape, out float3 center, out float height, out EulerAngles orientation
)
{
var capsule = shape.GetCapsuleProperties(out orientation);
return(capsule.BakeToBodySpace(shape.transform.localToWorldMatrix, shape.GetShapeToWorldMatrix(), out center, out height, ref orientation));
}
internal static void GetPlanePoints(
float3 center, float2 size, EulerAngles orientation,
out float3 vertex0, out float3 vertex1, out float3 vertex2, out float3 vertex3
)
{
var sizeYUp = math.float3(size.x, 0, size.y);
vertex0 = center + math.mul(orientation, sizeYUp * math.float3(-0.5f, 0, 0.5f));
vertex1 = center + math.mul(orientation, sizeYUp * math.float3(0.5f, 0, 0.5f));
vertex2 = center + math.mul(orientation, sizeYUp * math.float3(0.5f, 0, -0.5f));
vertex3 = center + math.mul(orientation, sizeYUp * math.float3(-0.5f, 0, -0.5f));
}
// matrix to transform point on a primitive from bake space into space of the shape
static float4x4 GetPrimitiveBakeToShapeMatrix(
float4x4 localToWorld, float4x4 shapeToWorld, ref float3 center, ref EulerAngles orientation, float3 scale, int3 basisPriority
)
{
if (
basisPriority.x == basisPriority.y ||
basisPriority.x == basisPriority.z ||
basisPriority.y == basisPriority.z
)
{
throw new ArgumentException(nameof(basisPriority));
}
var localToBasis = float4x4.TRS(center, orientation, scale);
// correct for imprecision in cases of no scale to prevent e.g., convex radius from being altered
if (scale.Equals(new float3(1f)))
{
localToBasis.c0 = math.normalizesafe(localToBasis.c0);
localToBasis.c1 = math.normalizesafe(localToBasis.c1);
localToBasis.c2 = math.normalizesafe(localToBasis.c2);
}
var localToBake = math.mul(localToWorld, localToBasis);
if (localToBake.HasNonUniformScale() || localToBake.HasShear())
{
// deskew second longest axis with respect to longest axis
localToBake[basisPriority[1]] =
DeskewSecondaryAxis(localToBake[basisPriority[0]], localToBake[basisPriority[1]]);
// recompute third axes from first two
var n2 = math.normalizesafe(
new float4(math.cross(localToBake[basisPriority[0]].xyz, localToBake[basisPriority[1]].xyz), 0f)
);
localToBake[basisPriority[2]] = n2 * math.dot(localToBake[basisPriority[2]], n2);
}
var bakeToShape = math.mul(math.inverse(shapeToWorld), localToBake);
// transform baked center/orientation (i.e. primitive basis) into shape space
orientation.SetValue(
quaternion.LookRotationSafe(bakeToShape[basisPriority[0]].xyz, bakeToShape[basisPriority[1]].xyz)
);
center = bakeToShape.c3.xyz;
return(bakeToShape);
}
internal static CapsuleGeometry GetBakedCapsuleProperties(
this PhysicsShapeAuthoring shape,
out float3 center, out float height, out EulerAngles orientation
)
{
var capsule = shape.GetCapsuleProperties(out orientation);
var radius = capsule.Radius;
center = capsule.GetCenter();
height = capsule.GetHeight();
var s = new float3(radius * 2f, radius * 2f, height);
var localToWorld = (float4x4)shape.transform.localToWorldMatrix;
var basisToWorld = GetBasisToWorldMatrix(localToWorld, center, orientation, s);
var basisPriority = k_DefaultAxisPriority;
var sheared = localToWorld.HasShear();
if (localToWorld.HasNonUniformScale() || sheared)
{
if (sheared)
{
basisPriority = GetBasisAxisPriority(basisToWorld);
}
MakeZAxisPrimaryBasis(ref basisPriority);
}
var bakeToShape = GetPrimitiveBakeToShapeMatrix(shape, ref center, ref orientation, 1f, basisPriority);
height *= math.length(bakeToShape.c2);
radius *= math.max(math.length(bakeToShape.c0), math.length(bakeToShape.c1));
var axis = math.mul(orientation, new float3 {
z = 1f
});
var endPoint = axis * math.max(0f, 0.5f * height - radius);
return(new CapsuleGeometry
{
Vertex0 = center + endPoint,
Vertex1 = center - endPoint,
Radius = radius
});
}
internal static BoxGeometry BakeToBodySpace(
this BoxGeometry box, float4x4 localToWorld, float4x4 shapeToWorld, EulerAngles orientation
)
{
using (var geometry = new NativeArray <BoxGeometry>(1, Allocator.TempJob)
{
[0] = box
})
{
var job = new BakeBoxJob
{
Box = geometry,
localToWorld = localToWorld,
shapeToWorld = shapeToWorld,
orientation = orientation
};
job.Run();
return(geometry[0]);
}
}
internal static void BakeToBodySpace(
float3 center, float2 size, EulerAngles orientation, float4x4 localToWorld, float4x4 shapeToWorld,
out float3 vertex0, out float3 vertex1, out float3 vertex2, out float3 vertex3
)
{
using (var geometry = new NativeArray <float3x4>(1, Allocator.TempJob))
{
var job = new BakePlaneJob
{
Vertices = geometry,
center = center,
size = size,
orientation = orientation,
localToWorld = localToWorld,
shapeToWorld = shapeToWorld
};
job.Run();
vertex0 = geometry[0].c0;
vertex1 = geometry[0].c1;
vertex2 = geometry[0].c2;
vertex3 = geometry[0].c3;
}
}
internal static SphereGeometry BakeToBodySpace(
this SphereGeometry sphere, float4x4 localToWorld, float4x4 shapeToWorld, ref EulerAngles orientation
)
{
using (var geometry = new NativeArray <SphereGeometry>(1, Allocator.TempJob)
{
[0] = sphere
})
using (var outOrientation = new NativeArray <EulerAngles>(1, Allocator.TempJob)
{
[0] = orientation
})
{
var job = new BakeSphereJob
{
Sphere = geometry,
Orientation = outOrientation,
localToWorld = localToWorld,
shapeToWorld = shapeToWorld
};
job.Run();
orientation = outOrientation[0];
return(geometry[0]);
}
}
internal static SphereGeometry GetBakedSphereProperties(this PhysicsShapeAuthoring shape, out EulerAngles orientation)
{
var sphere = shape.GetSphereProperties(out orientation);
return(sphere.BakeToBodySpace(shape.transform.localToWorldMatrix, shape.GetShapeToWorldMatrix(), ref orientation));
}
static float4x4 GetBakeToShape(float4x4 localToWorld, float4x4 shapeToWorld, ref float3 center, ref EulerAngles orientation)
{
var basisPriority = k_DefaultAxisPriority;
var sheared = localToWorld.HasShear();
if (localToWorld.HasNonUniformScale() || sheared)
{
if (sheared)
{
var transformScale = localToWorld.DecomposeScale();
var basisToWorld = GetBasisToWorldMatrix(localToWorld, center, orientation, transformScale);
basisPriority = GetBasisAxisPriority(basisToWorld);
}
MakeZAxisPrimaryBasis(ref basisPriority);
}
return(GetPrimitiveBakeToShapeMatrix(localToWorld, shapeToWorld, ref center, ref orientation, 1f, basisPriority));
}
public static float4x4 GetBakeToShape(PhysicsShapeAuthoring shape, float3 center, EulerAngles orientation)
{
var transform = shape.transform;
var localToWorld = (float4x4)transform.localToWorldMatrix;
var shapeToWorld = shape.GetShapeToWorldMatrix();
return(GetBakeToShape(localToWorld, shapeToWorld, ref center, ref orientation));
}
internal static CylinderGeometry BakeToBodySpace(
this CylinderGeometry cylinder, float4x4 localToWorld, float4x4 shapeToWorld, EulerAngles orientation
)
{
using (var geometry = new NativeArray <CylinderGeometry>(1, Allocator.TempJob)
{
[0] = cylinder
})
{
var job = new BakeCylinderJob
{
Cylinder = geometry,
localToWorld = localToWorld,
shapeToWorld = shapeToWorld,
orientation = orientation
};
job.Run();
return(geometry[0]);
}
}
static float4x4 GetBakeToShape(float4x4 localToWorld, float4x4 shapeToWorld, ref float3 center, ref EulerAngles orientation)
{
float4x4 bakeToShape;
float4x4 rotationMatrix = float4x4.identity;
var basisPriority = k_DefaultAxisPriority;
var sheared = localToWorld.HasShear();
if (localToWorld.HasNonUniformScale() || sheared)
{
if (sheared)
{
var transformScale = localToWorld.DecomposeScale();
var basisToWorld = GetBasisToWorldMatrix(localToWorld, center, orientation, transformScale);
basisPriority = GetBasisAxisPriority(basisToWorld);
}
rotationMatrix = new float4x4(
new float4 {
[basisPriority[2]] = 1
},
new float4 {
[basisPriority[1]] = 1
},
new float4 {
[basisPriority[0]] = 1
},
new float4 {
[3] = 1
}
);
}
bakeToShape =
GetPrimitiveBakeToShapeMatrix(localToWorld, shapeToWorld, ref center, ref orientation, 1f, basisPriority);
bakeToShape = math.mul(bakeToShape, rotationMatrix);
return(bakeToShape);
}
internal static SphereGeometry GetBakedSphereProperties(this PhysicsShapeAuthoring shape, out EulerAngles orientation)
{
var sphere = shape.GetSphereProperties(out orientation);
var center = sphere.Center;
var radius = sphere.Radius;
var basisToWorld = GetBasisToWorldMatrix(shape.transform.localToWorldMatrix, center, orientation, 1f);
var basisPriority = basisToWorld.HasShear() ? GetBasisAxisPriority(basisToWorld) : k_DefaultAxisPriority;
var bakeToShape = GetPrimitiveBakeToShapeMatrix(shape, ref center, ref orientation, 1f, basisPriority);
radius *= math.cmax(
new float3(math.length(bakeToShape.c0), math.length(bakeToShape.c1), math.length(bakeToShape.c2))
);
return(new SphereGeometry
{
Center = center,
Radius = radius
});
}
