public ColorIntensity RayIntensity(SDScene scene, RayData data)
{
ColorIntensity color = new ColorIntensity();
float lightPass = LightPassValue(scene.ClosestDistance, data.Pos);
if (lightPass == 0f)
{
return(color);
}
Vec3f dir = (Pos - data.Pos).Normalized();
float diffuseDot = dir.Dot(data.Normal);
if (diffuseDot > 0f)
{
color += data.Mat.DiffuseIntensity * DiffuseIntensity * diffuseDot * lightPass;
Vec3f reflectionDir = 2f * dir.Dot(data.Normal) * data.Normal - dir;
Vec3f viewerDir = (data.RayOrigin - data.Pos).Normalized();
float specularDot = reflectionDir.Dot(viewerDir);
if (specularDot > 0f)
{
color += SpecularIntensity * data.Mat.Specular
* MathF.Pow(specularDot, data.Mat.Shininess) * lightPass;
}
}
return(color.AtDistance((Pos - data.Pos).Magnitude));
}
private void UpdateData()
{
localCentroid = Vec3f.Zero;
Mass = 0;
for (int i = 0; i < colliders.Length; i++)
{
Collider col = colliders[i];
col.body = this;
Mass += col.mass;
localCentroid += col.mass * col.localCentroid;
}
InverseMass = 1 / Mass;
localCentroid *= InverseMass;
Mat3f localInertiaTensor = Mat3f.Zero;
for (int i = 0; i < colliders.Length; i++)
{
Collider col = colliders[i];
Vec3f r = localCentroid - col.localCentroid;
float rDotR = Vec3f.Dot(r, r);
Mat3f rOutR = Mat3f.OuterProduct(r, r);
localInertiaTensor = col.localInertiaTensor + col.mass * (rDotR * Mat3f.Identity - rOutR);
}
localInverseInertiaTensor = localInertiaTensor.Inverted();
}
public void TestNormals() {
var m = EasySquares(1);
var n1 = m.Normal(0, 1, 2);
var n2 = m.Normal(2, 1, 0);
Assert.AreEqual(Vec3f.Dot(n1, n2), -1.0f);
}
private bool Gjk(Collider colliderA, Collider colliderB, out ContactData contactData)
{
contactData = default;
Vec3f a, b, c, d;
Vec3f Asa, Asb, Asc, Asd, Bsa, Bsb, Bsc, Bsd;
a = b = c = d = Vec3f.Zero;
Asa = Asb = Asc = Asd = Vec3f.Zero;
Bsa = Bsb = Bsc = Bsd = Vec3f.Zero;
Vec3f searchDir = colliderA.localCentroid - colliderB.localCentroid;
CsoSupport(colliderB, colliderA, searchDir, out c, out Asc, out Bsc);
searchDir = -c;
CsoSupport(colliderB, colliderA, searchDir, out b, out Asb, out Bsb);
if (Vec3f.Dot(b, searchDir) < 0)
{
return(false);
}
Vec3f bc = c - b;
searchDir = Vec3f.Cross(Vec3f.Cross(bc, -b), bc); //search perpendicular to line segment towards origin
if (searchDir.SqrMagnitude < MathUtils.Epsilon)
{
searchDir = Vec3f.Cross(bc, Vec3f.Right); //normal with x-axis
if (Vec3f.Aproximates(searchDir, Vec3f.Zero))
{
searchDir = Vec3f.Cross(bc, -Vec3f.Forward); //normal with z-axis
}
}
int simplexDimension = 2;
for (int i = 0; i < 64; i++)
{
CsoSupport(colliderB, colliderA, searchDir, out a, out Asa, out Bsa);
if (Vec3f.Dot(a, searchDir) < 0)
{
return(false);
}
simplexDimension++;
if (simplexDimension == 3)
{
UpdateSimplex3(ref a, ref b, ref c, ref d, ref simplexDimension, ref searchDir, ref Asa, ref Asb, ref Asc, ref Asd
, ref Bsa, ref Bsb, ref Bsc, ref Bsd);
}
else if (UpdateSimplex4(ref a, ref b, ref c, ref d, ref simplexDimension, ref searchDir, ref Asa, ref Asb, ref Asc, ref Asd
, ref Bsa, ref Bsb, ref Bsc, ref Bsd))
{
contactData = Epa(colliderA, colliderB, a, b, c, d, Asa, Asb, Asc, Asd, Bsa, Bsb, Bsc, Bsd);
return(true);
}
}
return(false);
}
public static Quatf operator *(Quatf lhs, Quatf rhs)
{
Vec3f lxyz = lhs.XYZ;
Vec3f rxyz = rhs.XYZ;
return(new Quatf(
(rhs.w * lxyz) + (lhs.w * rxyz) + Vec3f.Cross(lxyz, rxyz),
(lhs.w * rhs.w) - Vec3f.Dot(lxyz, rxyz)));
}
public IEnumerable <List <int> > AllPolygons()
{
HashSet <int> foundTris = new HashSet <int>();
Queue <int> triQueue = new Queue <int>();
for (int i = 0; i < Edges.Length; i += 3)
{
if (Edges[i].vertexIdx < 0 || foundTris.Contains(i))
{
continue;
}
List <int> currPoly = new List <int>();
Vec3f n = Normal(i);
triQueue.Enqueue(i);
while (triQueue.Count > 0)
{
int e = triQueue.Dequeue();
if (foundTris.Contains(e))
{
// Already processed
continue;
}
if (Vec3f.Dot(n, Normal(e)) < 0.99f)
{
// Creased edge; not part of the same polygon.
continue;
}
foundTris.Add(e);
currPoly.Add(e);
foreach (int nextE in new int[] { Opp(e), Opp(Next(e)), Opp(Prev(e)) })
{
if (nextE < 0)
{
// Open edge
continue;
}
int nextEBase = Base(nextE);
triQueue.Enqueue(nextEBase);
}
}
yield return(currPoly);
}
}
public bool IsCreased(int edgeIdx)
{
if (IsOpen(edgeIdx))
{
return(false);
}
Vec3f n1 = Normal(edgeIdx);
Vec3f n2 = Normal(Opp(edgeIdx));
return(Vec3f.Dot(n1, n2) < 0.99f);
}
public static bool ContainsPoint(Vec3f a, Vec3f b, Vec3f c, Vec3f point)
{
a -= point;
b -= point;
c -= point;
Vec3f u = Vec3f.Cross(b, c);
Vec3f v = Vec3f.Cross(c, a);
Vec3f w = Vec3f.Cross(a, b);
return(Vec3f.Dot(u, v) >= 0f && Vec3f.Dot(u, w) > 0f);
}
public bool Flips(int vert1, int vert2, int vertOld, int vertNew)
{
Vec3f n1 = Normal(vert1, vert2, vertOld);
Vec3f n2 = Normal(vert1, vert2, vertNew);
if (float.IsNaN(n2.x))
{
return(true);
}
//return Vec3f.Dot(n1, n2) <= 0f;
return(Vec3f.Dot(n1, n2) <= 0.01f);
}
public IEnumerable <int> FullPolygon(int startEdge)
{
HashSet <int> foundTris = new HashSet <int>();
Queue <int> triQueue = new Queue <int>();
Vec3f n = Normal(startEdge);
foundTris.Add(Base(startEdge));
triQueue.Enqueue(Base(startEdge));
while (triQueue.Count > 0)
{
int e = triQueue.Dequeue();
if (Vec3f.Dot(n, Normal(e)) < 0.99f)
{
// Creased edge; not part of the same polygon.
continue;
}
yield return(e);
foreach (int nextE in new int[] { Opp(e), Opp(Next(e)), Opp(Prev(e)) })
{
if (nextE < 0)
{
// Open edge
continue;
}
int nextEBase = Base(nextE);
if (foundTris.Contains(nextEBase))
{
// All processed triangle
continue;
}
foundTris.Add(nextEBase);
triQueue.Enqueue(nextEBase);
}
}
}
public Vec3f GetBarycentric(Vec3f point)
{
Vec3f v0 = b - a;
Vec3f v1 = c - a;
Vec3f v2 = point - a;
float d00 = Vec3f.Dot(v0, v0);
float d01 = Vec3f.Dot(v0, v1);
float d11 = Vec3f.Dot(v1, v1);
float d20 = Vec3f.Dot(v2, v0);
float d21 = Vec3f.Dot(v2, v1);
float denom = d00 * d11 - d01 * d01;
float v = (d11 * d20 - d01 * d21) / denom;
float w = (d00 * d21 - d01 * d20) / denom;
float u = 1.0f - v - w;
return(new Vec3f(u, v, w));
}
public DirectionalColorData DirectionalColor(Vec3f start, Vec3f direction, int reflections)
{
Vec3f pos = start;
for (int i = 0; i < MaxMarches; i++)
{
var objData = ClosestObject(pos);
if (objData.Distance <= DistanceThreshold)
{
Vec3f normal = Gradient(pos);
pos += 2f * DistanceThreshold * normal;
var data = new RayData(pos, start, normal, objData.SDObj.Mat);
ColorIntensity intensity = Lighting.RayIntensity(data);
if (reflections > 0 && objData.SDObj.Mat.Specular > 0f)
{
Vec3f reflectionDir = direction - 2f * normal.Dot(direction) * normal;
intensity += DirectionalColor(pos, reflectionDir, reflections - 1).Intensity
*objData.SDObj.Mat.Specular;
}
return(new DirectionalColorData
{
Intensity = intensity
});
}
if ((pos - Cam.Pos).Magnitude > MaxRayDistance)
{
return(Lighting.Background.BackgroundColorData(direction));
}
pos += direction * objData.Distance;
}
return(Lighting.Background.BackgroundColorData(direction));
}
public static Vec3f ClosestPointToPoint(Vec3f lineA, Vec3f lineB, Vec3f point)
{
Vec3f ab = lineB - lineA;
Vec3f ap = point - lineA;
float atbsqr = ab.SqrMagnitude;
float dot = Vec3f.Dot(ab, ap);
float dist = dot / atbsqr;
if (dist < 0)
{
return(lineA);
}
else if (dist > 1)
{
return(lineB);
}
else
{
return(lineA + ab * dist);
}
}
public bool SameDirEdges(int edge1, int edge2)
{
Vec3f d1 = GetEdgeDelta(edge1);
Vec3f d2 = GetEdgeDelta(edge2);
if (Vec3f.Dot(d1, d2) < 0)
{
return(false);
}
float c = Vec3f.Cross(d1, d2).MagnitudeSqrd();
const float kEdgeThresh = 0.01f;
if (c >= kEdgeThresh * kEdgeThresh)
{
return(false);
}
else
{
return(true);
}
}
public Plane(Vec3f a, Vec3f b, Vec3f c)
{
normal = Vec3f.Cross(a - b, a - c).Normalized();
distance = Vec3f.Dot(normal, a);
}
public bool CanCollapse(int edgeIdx)
{
var srcType = SrcType(edgeIdx);
var destType = DestType(edgeIdx);
//return !IsCorner(edgeIdx);
if (srcType == VertexType.PerimeterCorner)
{
ignore1++;
return(false);
}
if (srcType == VertexType.PerimeterStraight && destType == VertexType.Interior)
{
ignore2++;
return(false);
}
/*if (srcType != VertexType.Interior && destType != VertexType.Interior
* && !IsOpen(edgeIdx)) {
* // Can not connect open edges by a closed edge
* ignore3++;
* return false;
* }*/
/*if (srcType != VertexType.Interior && destType != VertexType.Interior
* && !IsCreased(edgeIdx)) {
* // Can not connect open edges by a closed edge
* ignore3++;
* return false;
* }*/
if (srcType != VertexType.Interior && destType != VertexType.Interior
//&& GetOpenOutEdgeSrc(edgeIdx) != edgeIdx) {
&& (GetOpenInEdgeDest(edgeIdx) != edgeIdx || GetOpenOutEdgeSrc(edgeIdx) != edgeIdx))
{
// Can not connect open edges by a closed edge
ignore3++;
return(false);
}
if (srcType != VertexType.Interior && destType != VertexType.Interior &&
!SameDirEdges(GetOpenOutEdgeSrc(edgeIdx), GetOpenInEdgeSrc(edgeIdx)))
{
// Can not connect open edges by a closed edge
ignore3++;
return(false);
}
if (srcType != VertexType.Interior && destType != VertexType.Interior &&
!SameDirEdges(edgeIdx, GetOpenInEdgeSrc(edgeIdx)))
{
// Can not connect open edges by a closed edge
ignore3++;
return(false);
}
foreach (var e1 in OpenOrCreasedEdges(Prev(edgeIdx)))
{
foreach (var e2 in OpenOrCreasedEdges(Prev(edgeIdx)))
{
//if (Math.Abs(Vec3f.Dot(GetEdgeDelta(e1), GetEdgeDelta(e2))) < 0.01f) {
if (Math.Abs(Vec3f.Dot(GetEdgeDelta(e1), GetEdgeDelta(e2))) < 0.01f)
{
// Open or creased edges that are orthogonal; stop.
return(false);
}
}
}
int vIdxOld = Edges[Prev(edgeIdx)].vertexIdx;
int vIdxNew = Edges[edgeIdx].vertexIdx;
// Checks if the collapse would put points on the wrong side of line segments
/*foreach (var e1 in IncomingEdges(Prev(edgeIdx))) {
* if (e1 == Prev(edgeIdx) || e1 == Opp(edgeIdx)) {
* // Ignore triangles that are about to be removed
* continue;
* }
* int from = Edges[Prev(e1)].vertexIdx;
*
* foreach (var e2 in IncomingEdges(Prev(edgeIdx))) {
* //foreach (var e2 in IncomingEdges(Prev(e1))) {
* if (InsideTri(from, vIdxOld, vIdxNew, Edges[Prev(e2)].vertexIdx)) {
* return false;
* }
* }
* }*/
// Check if this merge would flip any triangles
foreach (var e in IncomingEdges(Prev(edgeIdx)))
{
if (e == Prev(edgeIdx) || e == Opp(edgeIdx))
{
// Ignore triangles that are about to be removed
continue;
}
if (Flips(Edges[Prev(e)].vertexIdx, Edges[Next(e)].vertexIdx, vIdxOld, vIdxNew))
{
return(false);
}
}
/*if (srcType != VertexType.Interior && destType != VertexType.Interior
* && !SameDirEdges(edgeIdx, GetOpenOutEdgeDest(edgeIdx))) {
* // Can not connect open edges by a closed edge
* ignore3++;
* return false;
* }*/
int openCount = 0;
foreach (var startEdge in new int[] { edgeIdx, Opp(edgeIdx) })
{
int localOpenCount = 0;
if (startEdge < 0)
{
continue;
}
int e = startEdge;
for (int i = 0; i < 3; ++i)
{
if (IsOpen(e) || IsCreased(e))
{
openCount++;
localOpenCount++;
}
e = Next(e);
}
if (localOpenCount >= 2)
{
//return false;
}
}
/*if (openCount > 2) {
* return false;
* } else if (openCount == 2) {
* return srcType == VertexType.PerimeterStraight;
* } else {
* return true;
* }*/
return(true);
//return false;
}
public Plane(Vec3f point, Vec3f normal)
{
this.normal = normal;
distance = Vec3f.Dot(normal, point);
}
public void CullInvisibleChunks()
{
if (!ClientSettings.Occlusionculling || game.WorldMap.chunks.Count < 100)
{
return;
}
Vec3d camPos = game.player.Entity.CameraPos;
centerpos.Set((int)(camPos.X / chunksize), (int)(camPos.Y / chunksize), (int)(camPos.Z / chunksize));
isAboveHeightLimit = centerpos.Y >= game.WorldMap.ChunkMapSizeY;
playerViewVec = EntityPos.GetViewVector(game.mousePitch, game.mouseYaw).Normalize();
lock (game.WorldMap.chunksLock)
{
foreach (var val in game.WorldMap.chunks)
{
val.Value.SetVisible(false);
}
// We sometimes have issues with chunks adjacent to the player getting culled, so lets make these always visible
for (int dx = -1; dx <= 1; dx++)
{
for (int dy = -1; dy <= 1; dy++)
{
for (int dz = -1; dz <= 1; dz++)
{
long index3d = game.WorldMap.ChunkIndex3D(dx + centerpos.X, dy + centerpos.Y, dz + centerpos.Z);
ClientChunk chunk = null;
if (game.WorldMap.chunks.TryGetValue(index3d, out chunk))
{
chunk.SetVisible(true);
}
}
}
}
}
// Add some 15 extra degrees to the field of view angle for safety
float fov = GameMath.Cos(game.MainCamera.Fov + 15 * GameMath.DEG2RAD);
for (int i = 0; i < cubicShellPositions.Length; i++)
{
Vec3i vec = cubicShellPositions[i];
float dotProd = playerViewVec.Dot(cubicShellPositionsNormalized[i]);
if (dotProd <= fov / 2)
{
// Outside view frustum
continue;
}
// It seems that one trace can cause issues where chunks are culled when they shouldn't
// 2 traces with a y-offset seems to mitigate most of that issue
TraverseRayAndMarkVisible(centerpos, vec, 0.25);
TraverseRayAndMarkVisible(centerpos, vec, 0.75);
}
}
public static float SignedDistanceToPoint(float distance, Vec3f planeNormal, Vec3f point)
{
return(Vec3f.Dot(planeNormal, point - (planeNormal * distance)));
}
private ContactData Epa(Collider colliderA, Collider colliderB, Vec3f a, Vec3f b, Vec3f c, Vec3f d,
Vec3f Asa, Vec3f Asb, Vec3f Asc, Vec3f Asd, Vec3f Bsa, Vec3f Bsb, Vec3f Bsc, Vec3f Bsd)
{
List <FaceData> faces = new List <FaceData>
{
new FaceData {
triangle = new Triangle(a, b, c), aSupport = new Triangle(Asa, Asb, Asc), bSupport = new Triangle(Bsa, Bsb, Bsc)
},
new FaceData {
triangle = new Triangle(a, c, d), aSupport = new Triangle(Asa, Asc, Asd), bSupport = new Triangle(Bsa, Bsc, Bsd)
},
new FaceData {
triangle = new Triangle(a, d, b), aSupport = new Triangle(Asa, Asd, Asb), bSupport = new Triangle(Bsa, Bsd, Bsb)
},
new FaceData {
triangle = new Triangle(b, d, c), aSupport = new Triangle(Asb, Asd, Asc), bSupport = new Triangle(Bsb, Bsd, Bsc)
},
};
FaceData prevFace = faces[0];
for (int maxIt = 0; maxIt < 64; maxIt++)
{
int closestFaceIndex = GetClosestTriangle(faces, out float distance, out Vec3f projectedPoint);
Vec3f searchDir = faces[closestFaceIndex].triangle.GetNormal();
CsoSupport(colliderA, colliderB, searchDir, out Vec3f support, out Vec3f supportA, out Vec3f supportB);
//If closest is no closer than previous face
if (distance >= prevFace.triangle.ClosestPointToPoint(Vec3f.Zero).SqrMagnitude)
{
FaceData closestFace = faces[closestFaceIndex];
Vec3f barycentric = closestFace.triangle.GetBarycentric(projectedPoint);
Vec3f localA = closestFace.aSupport.a * barycentric.x + closestFace.aSupport.b * barycentric.y + closestFace.aSupport.c * barycentric.z;
Vec3f localB = closestFace.bSupport.a * barycentric.x + closestFace.bSupport.b * barycentric.y + closestFace.bSupport.c * barycentric.z;
return(new ContactData(colliderA.body.LocalToGlobalPos(localA), localA, colliderB.body.LocalToGlobalPos(localB)
, localB, searchDir, distance));
}
prevFace = faces[closestFaceIndex];
List <LineData> edgesToFix = new List <LineData>();
for (int i = 0; i < faces.Count; i++)
{
FaceData currFace = faces[i];
//Remove face and add edges be fixed. Remove if they are already in the list since it doesn't have any faces conected.
if (Vec3f.Dot(currFace.triangle.GetNormal(), support - currFace.triangle.a) > 0)
{
LineData ab = new LineData()
{
line = currFace.triangle.AB, aSup = currFace.aSupport.AB, bSup = currFace.bSupport.AB
};
if (edgesToFix.Contains(ab))
{
edgesToFix.Remove(ab);
}
else
{
edgesToFix.Add(ab);
}
LineData ac = new LineData()
{
line = currFace.triangle.AC, aSup = currFace.aSupport.AC, bSup = currFace.bSupport.AC
};
if (edgesToFix.Contains(ac))
{
edgesToFix.Remove(ac);
}
else
{
edgesToFix.Add(ac);
}
LineData bc = new LineData()
{
line = currFace.triangle.BC, aSup = currFace.aSupport.BC, bSup = currFace.bSupport.BC
};
if (edgesToFix.Contains(bc))
{
edgesToFix.Remove(bc);
}
else
{
edgesToFix.Add(bc);
}
faces.RemoveAt(i);
i--;
}
}
for (int i = 0; i < edgesToFix.Count; i++)
{
LineData currEdge = edgesToFix[i];
FaceData face = new FaceData()
{
triangle = new Triangle(currEdge.line.a, currEdge.line.b, support),
aSupport = new Triangle(currEdge.aSup.a, currEdge.aSup.b, supportA),
bSupport = new Triangle(currEdge.bSup.a, currEdge.bSup.b, supportB),
};
if (Vec3f.Dot(face.triangle.a, face.triangle.GetNormal()) + MathUtils.Epsilon < 0)
{
Vec3f temp = face.triangle.a;
face.triangle.a = face.triangle.b;
face.triangle.b = temp;
temp = face.aSupport.a;
face.aSupport.a = face.aSupport.b;
face.aSupport.b = temp;
temp = face.bSupport.a;
face.bSupport.a = face.bSupport.b;
face.bSupport.b = temp;
}
faces.Add(face);
}
}
return(default);