public static Vector3 SnapDeltaToGrid(Camera camera, Vector3 worldDeltaMovement, Vector3[] worldPoints, bool snapToGridPlane = true, bool snapToSelf = false)
{
UpdateGridOrientation(camera);
if (gridOrientation == null || worldPoints == null || worldPoints.Length == 0)
{
return(worldDeltaMovement);
}
var worldPlane = gridOrientation.gridWorkPlane;
var scaleVector = gridOrientation.gridSnapScale;
var snapVector = gridOrientation.gridSnapVector;
var gridLocalDeltaMovement = VectorToGridSpace(worldDeltaMovement);
var gridLocalPlane = PlaneToGridSpace(worldPlane);
if (snapToGridPlane)
{
scaleVector.x *= (Mathf.Abs(gridLocalPlane.a) >= 1 - MathConstants.EqualityEpsilon) ? 0 : 1;
scaleVector.y *= (Mathf.Abs(gridLocalPlane.b) >= 1 - MathConstants.EqualityEpsilon) ? 0 : 1;
scaleVector.z *= (Mathf.Abs(gridLocalPlane.c) >= 1 - MathConstants.EqualityEpsilon) ? 0 : 1;
}
var snappedDeltaMovement = gridLocalDeltaMovement;
if (Mathf.Abs(scaleVector.x) < MathConstants.EqualityEpsilon)
{
snappedDeltaMovement.x = 0;
}
if (Mathf.Abs(scaleVector.y) < MathConstants.EqualityEpsilon)
{
snappedDeltaMovement.y = 0;
}
if (Mathf.Abs(scaleVector.z) < MathConstants.EqualityEpsilon)
{
snappedDeltaMovement.z = 0;
}
Vector3[] gridLocalPoints;
if (worldPoints.Length > 1)
{
var bounds = new AABB();
bounds.Reset();
for (int i = 0; i < worldPoints.Length; i++)
{
Vector3 localPoint = PointToGridSpace(worldPoints[i]);
if (snapToGridPlane)
{
localPoint = GeometryUtility.ProjectPointOnPlane(gridLocalPlane, localPoint);
}
if (float.IsNaN(localPoint.x) || float.IsNaN(localPoint.y) || float.IsNaN(localPoint.z) ||
float.IsInfinity(localPoint.x) || float.IsInfinity(localPoint.y) || float.IsInfinity(localPoint.z))
{
continue;
}
bounds.Extend(localPoint);
}
gridLocalPoints = bounds.GetCorners();
}
else
{
var localGridSpacePoint = PointToGridSpace(worldPoints[0]);
Vector3 projectedPoint = localGridSpacePoint;
if (snapToGridPlane)
{
projectedPoint = GeometryUtility.ProjectPointOnPlane(gridLocalPlane, localGridSpacePoint);
}
if (float.IsNaN(projectedPoint.x) || float.IsNaN(projectedPoint.y) || float.IsNaN(projectedPoint.z) ||
float.IsInfinity(projectedPoint.x) || float.IsInfinity(projectedPoint.y) || float.IsInfinity(projectedPoint.z))
{
gridLocalPoints = new Vector3[0] {
}
}
;
else
{
gridLocalPoints = new Vector3[] { projectedPoint }
};
}
for (int i = 0; i < gridLocalPoints.Length; i++)
{
var oldPoint = gridLocalPoints[i];
var newPoint = gridLocalPoints[i] + gridLocalDeltaMovement;
if (snapToGridPlane)
{
newPoint = GeometryUtility.ProjectPointOnPlane(gridLocalPlane, newPoint);
}
newPoint = GridUtility.CleanPosition(newPoint);
var snappedNewPoint = SnapRoundPosition(newPoint, snapVector);
if (snapToGridPlane)
{
snappedNewPoint = GeometryUtility.ProjectPointOnPlane(gridLocalPlane, snappedNewPoint);
}
snappedNewPoint = GridUtility.CleanPosition(snappedNewPoint);
var foundDeltaMovement = (snappedNewPoint - oldPoint);
foundDeltaMovement.x *= scaleVector.x;
foundDeltaMovement.y *= scaleVector.y;
foundDeltaMovement.z *= scaleVector.z;
if (i == 0 || Math.Abs(foundDeltaMovement.x) < Mathf.Abs(snappedDeltaMovement.x))
{
snappedDeltaMovement.x = foundDeltaMovement.x;
}
if (i == 0 || Math.Abs(foundDeltaMovement.y) < Mathf.Abs(snappedDeltaMovement.y))
{
snappedDeltaMovement.y = foundDeltaMovement.y;
}
if (i == 0 || Math.Abs(foundDeltaMovement.z) < Mathf.Abs(snappedDeltaMovement.z))
{
snappedDeltaMovement.z = foundDeltaMovement.z;
}
}
if (snapToSelf)
{
var snapDelta = (snappedDeltaMovement - gridLocalDeltaMovement);
if (Mathf.Abs(snapDelta.x) > Mathf.Abs(gridLocalDeltaMovement.x))
{
snappedDeltaMovement.x = 0;
}
if (Mathf.Abs(snapDelta.y) > Mathf.Abs(gridLocalDeltaMovement.y))
{
snappedDeltaMovement.y = 0;
}
if (Mathf.Abs(snapDelta.z) > Mathf.Abs(gridLocalDeltaMovement.z))
{
snappedDeltaMovement.z = 0;
}
}
worldDeltaMovement = VectorFromGridSpace(snappedDeltaMovement);
return(worldDeltaMovement);
}
public static Vector3 SnapDeltaToRayGrid(Camera camera, Ray worldRay, Vector3 worldDeltaMovement, Vector3[] worldPoints, bool snapToSelf = false)
{
UpdateGridOrientation(camera);
if (gridOrientation == null || worldPoints == null || worldPoints.Length == 0)
{
return(worldDeltaMovement);
}
var snapVector = gridOrientation.gridSnapVector;
var scaleVector = gridOrientation.gridSnapScale;
var localDeltaMovement = VectorToGridSpace(worldDeltaMovement);
var localLineDir = VectorToGridSpace(worldRay.direction);
var localLineOrg = PointToGridSpace(worldRay.origin);
scaleVector.x *= ((Mathf.Abs(localLineDir.y) >= 1 - MathConstants.EqualityEpsilon) || (Mathf.Abs(localLineDir.z) >= 1 - MathConstants.EqualityEpsilon)) ? 0 : 1;
scaleVector.y *= ((Mathf.Abs(localLineDir.x) >= 1 - MathConstants.EqualityEpsilon) || (Mathf.Abs(localLineDir.z) >= 1 - MathConstants.EqualityEpsilon)) ? 0 : 1;
scaleVector.z *= ((Mathf.Abs(localLineDir.x) >= 1 - MathConstants.EqualityEpsilon) || (Mathf.Abs(localLineDir.y) >= 1 - MathConstants.EqualityEpsilon)) ? 0 : 1;
var snappedDeltaMovement = localDeltaMovement;
if (Mathf.Abs(scaleVector.x) < MathConstants.EqualityEpsilon)
{
snappedDeltaMovement.x = 0;
}
if (Mathf.Abs(scaleVector.y) < MathConstants.EqualityEpsilon)
{
snappedDeltaMovement.y = 0;
}
if (Mathf.Abs(scaleVector.z) < MathConstants.EqualityEpsilon)
{
snappedDeltaMovement.z = 0;
}
Vector3[] localPoints;
if (worldPoints.Length > 1)
{
var bounds = new AABB();
bounds.Reset();
for (int i = 0; i < worldPoints.Length; i++)
{
var localPoint = GeometryUtility.ProjectPointOnInfiniteLine(PointToGridSpace(worldPoints[i]), localLineOrg, localLineDir);
bounds.Extend(localPoint);
}
localPoints = bounds.GetCorners();
}
else
{
localPoints = new Vector3[] { GeometryUtility.ProjectPointOnInfiniteLine(PointToGridSpace(worldPoints[0]), localLineOrg, localLineDir) };
}
for (int i = 0; i < localPoints.Length; i++)
{
var oldPoint = localPoints[i];
var newPoint = GeometryUtility.ProjectPointOnInfiniteLine(oldPoint + localDeltaMovement, localLineOrg, localLineDir);
var snappedNewPoint = SnapRoundPosition(newPoint, snapVector);
snappedNewPoint = GridUtility.CleanPosition(GeometryUtility.ProjectPointOnInfiniteLine(snappedNewPoint, localLineOrg, localLineDir));
var foundDeltaMovement = (snappedNewPoint - oldPoint);
foundDeltaMovement.x *= scaleVector.x;
foundDeltaMovement.y *= scaleVector.y;
foundDeltaMovement.z *= scaleVector.z;
if (i == 0 || Math.Abs(foundDeltaMovement.x) < Mathf.Abs(snappedDeltaMovement.x))
{
snappedDeltaMovement.x = foundDeltaMovement.x;
}
if (i == 0 || Math.Abs(foundDeltaMovement.y) < Mathf.Abs(snappedDeltaMovement.y))
{
snappedDeltaMovement.y = foundDeltaMovement.y;
}
if (i == 0 || Math.Abs(foundDeltaMovement.z) < Mathf.Abs(snappedDeltaMovement.z))
{
snappedDeltaMovement.z = foundDeltaMovement.z;
}
}
if (snapToSelf)
{
var snapDelta = (snappedDeltaMovement - localDeltaMovement);
if (Mathf.Abs(snapDelta.x) > Mathf.Abs(localDeltaMovement.x))
{
snappedDeltaMovement.x = 0;
}
if (Mathf.Abs(snapDelta.y) > Mathf.Abs(localDeltaMovement.y))
{
snappedDeltaMovement.y = 0;
}
if (Mathf.Abs(snapDelta.z) > Mathf.Abs(localDeltaMovement.z))
{
snappedDeltaMovement.z = 0;
}
}
worldDeltaMovement = VectorFromGridSpace(snappedDeltaMovement);
return(worldDeltaMovement);
}
public static Vector3 SnapLocalPointToWorldGridDelta(Camera camera, Matrix4x4 pointLocalToWorld, Matrix4x4 pointWorldToLocal, Vector3[] localPoints)
{
UpdateGridOrientation(camera);
if (gridOrientation == null || localPoints == null || localPoints.Length == 0)
{
return(Vector3.zero);
}
var worldToGridLocal = Matrix4x4.TRS(-gridOrientation.gridWorkCenter, Quaternion.identity, Vector3.one) * ToGridSpaceMatrix();
var gridLocalToWorld = FromGridSpaceMatrix() * Matrix4x4.TRS(gridOrientation.gridWorkCenter, Quaternion.identity, Vector3.one);
//var gridLocalToPointLocal = gridLocalToWorld * pointWorldToLocal;
var pointLocalToGridLocal = pointLocalToWorld * worldToGridLocal;
Vector3[] gridLocalPoints;
if (localPoints.Length > 1)
{
var bounds = new AABB();
bounds.Reset();
for (int i = 0; i < localPoints.Length; i++)
{
Vector3 localPoint = pointLocalToGridLocal.MultiplyPoint(localPoints[i]);
if (float.IsNaN(localPoint.x) || float.IsNaN(localPoint.y) || float.IsNaN(localPoint.z) ||
float.IsInfinity(localPoint.x) || float.IsInfinity(localPoint.y) || float.IsInfinity(localPoint.z))
{
continue;
}
bounds.Extend(localPoint);
}
gridLocalPoints = bounds.GetCorners();
}
else
{
var localGridSpacePoint = pointLocalToGridLocal.MultiplyPoint(localPoints[0]);
if (float.IsNaN(localGridSpacePoint.x) || float.IsNaN(localGridSpacePoint.y) || float.IsNaN(localGridSpacePoint.z) ||
float.IsInfinity(localGridSpacePoint.x) || float.IsInfinity(localGridSpacePoint.y) || float.IsInfinity(localGridSpacePoint.z))
{
gridLocalPoints = new Vector3[0] {
}
}
;
else
{
gridLocalPoints = new Vector3[] { localGridSpacePoint }
};
}
var snappedDeltaMovement = Vector3.zero;
var snapVector = gridOrientation.gridSnapVector;
for (int i = 0; i < gridLocalPoints.Length; i++)
{
var foundDeltaMovement = DeltaSnapRoundPosition(gridLocalPoints[i], snapVector);
if (i == 0 || Math.Abs(foundDeltaMovement.x) < Mathf.Abs(snappedDeltaMovement.x))
{
snappedDeltaMovement.x = foundDeltaMovement.x;
}
if (i == 0 || Math.Abs(foundDeltaMovement.y) < Mathf.Abs(snappedDeltaMovement.y))
{
snappedDeltaMovement.y = foundDeltaMovement.y;
}
if (i == 0 || Math.Abs(foundDeltaMovement.z) < Mathf.Abs(snappedDeltaMovement.z))
{
snappedDeltaMovement.z = foundDeltaMovement.z;
}
}
var scaleVector = gridOrientation.gridSnapScale;
snappedDeltaMovement.x *= scaleVector.x;
snappedDeltaMovement.y *= scaleVector.y;
snappedDeltaMovement.z *= scaleVector.z;
var worldDeltaMovement = gridLocalToWorld.MultiplyVector(snappedDeltaMovement);
return(worldDeltaMovement);
}
