public static FMat4 FromTRS(FVec3 pos, FQuat q, FVec3 scale)
{
FMat4 m = FromScale(scale) * FromQuaternion(q);
m.w.x = pos.x;
m.w.y = pos.y;
m.w.z = pos.z;
return(m);
}
public static FMat4 Transpose(FMat4 m)
{
return(new FMat4
(
new FVec4(m.x.x, m.y.x, m.z.x, m.w.x),
new FVec4(m.x.y, m.y.y, m.z.y, m.w.y),
new FVec4(m.x.z, m.y.z, m.z.z, m.w.z),
new FVec4(m.x.w, m.y.w, m.z.w, m.w.w)
));
}
public static FMat4 Invert(FMat4 m)
{
Fix64 determinant = Fix64.One / m.Determinant();
FMat4 mat = new FMat4
(
new FVec4
(
(m.y.y * m.z.z * m.w.w + m.y.z * m.z.w * m.w.y + m.y.w * m.z.y * m.w.z -
m.y.y * m.z.w * m.w.z - m.y.z * m.z.y * m.w.w - m.y.w * m.z.z * m.w.y) * determinant,
(m.x.y * m.z.w * m.w.z + m.x.z * m.z.y * m.w.w + m.x.w * m.z.z * m.w.y -
m.x.y * m.z.z * m.w.w - m.x.z * m.z.w * m.w.y - m.x.w * m.z.y * m.w.z) * determinant,
(m.x.y * m.y.z * m.w.w + m.x.z * m.y.w * m.w.y + m.x.w * m.y.y * m.w.z -
m.x.y * m.y.w * m.w.z - m.x.z * m.y.y * m.w.w - m.x.w * m.y.z * m.w.y) * determinant,
(m.x.y * m.y.w * m.z.z + m.x.z * m.y.y * m.z.w + m.x.w * m.y.z * m.z.y -
m.x.y * m.y.z * m.z.w - m.x.z * m.y.w * m.z.y - m.x.w * m.y.y * m.z.z) * determinant
),
new FVec4
(
(m.y.x * m.z.w * m.w.z + m.y.z * m.z.x * m.w.w + m.y.w * m.z.z * m.w.x -
m.y.x * m.z.z * m.w.w - m.y.z * m.z.w * m.w.x - m.y.w * m.z.x * m.w.z) * determinant,
(m.x.x * m.z.z * m.w.w + m.x.z * m.z.w * m.w.x + m.x.w * m.z.x * m.w.z -
m.x.x * m.z.w * m.w.z - m.x.z * m.z.x * m.w.w - m.x.w * m.z.z * m.w.x) * determinant,
(m.x.x * m.y.w * m.w.z + m.x.z * m.y.x * m.w.w + m.x.w * m.y.z * m.w.x -
m.x.x * m.y.z * m.w.w - m.x.z * m.y.w * m.w.x - m.x.w * m.y.x * m.w.z) * determinant,
(m.x.x * m.y.z * m.z.w + m.x.z * m.y.w * m.z.x + m.x.w * m.y.x * m.z.z -
m.x.x * m.y.w * m.z.z - m.x.z * m.y.x * m.z.w - m.x.w * m.y.z * m.z.x) * determinant
),
new FVec4
(
(m.y.x * m.z.y * m.w.w + m.y.y * m.z.w * m.w.x + m.y.w * m.z.x * m.w.y -
m.y.x * m.z.w * m.w.y - m.y.y * m.z.x * m.w.w - m.y.w * m.z.y * m.w.x) * determinant,
(m.x.x * m.z.w * m.w.y + m.x.y * m.z.x * m.w.w + m.x.w * m.z.y * m.w.x -
m.x.x * m.z.y * m.w.w - m.x.y * m.z.w * m.w.x - m.x.w * m.z.x * m.w.y) * determinant,
(m.x.x * m.y.y * m.w.w + m.x.y * m.y.w * m.w.x + m.x.w * m.y.x * m.w.y -
m.x.x * m.y.w * m.w.y - m.x.y * m.y.x * m.w.w - m.x.w * m.y.y * m.w.x) * determinant,
(m.x.x * m.y.w * m.z.y + m.x.y * m.y.x * m.z.w + m.x.w * m.y.y * m.z.x -
m.x.x * m.y.y * m.z.w - m.x.y * m.y.w * m.z.x - m.x.w * m.y.x * m.z.y) * determinant
),
new FVec4
(
(m.y.x * m.z.z * m.w.y + m.y.y * m.z.x * m.w.z + m.y.z * m.z.y * m.w.x -
m.y.x * m.z.y * m.w.z - m.y.y * m.z.z * m.w.x - m.y.z * m.z.x * m.w.y) * determinant,
(m.x.x * m.z.y * m.w.z + m.x.y * m.z.z * m.w.x + m.x.z * m.z.x * m.w.y -
m.x.x * m.z.z * m.w.y - m.x.y * m.z.x * m.w.z - m.x.z * m.z.y * m.w.x) * determinant,
(m.x.x * m.y.z * m.w.y + m.x.y * m.y.x * m.w.z + m.x.z * m.y.y * m.w.x -
m.x.x * m.y.y * m.w.z - m.x.y * m.y.z * m.w.x - m.x.z * m.y.x * m.w.y) * determinant,
(m.x.x * m.y.y * m.z.z + m.x.y * m.y.z * m.z.x + m.x.z * m.y.x * m.z.y -
m.x.x * m.y.z * m.z.y - m.x.y * m.y.x * m.z.z - m.x.z * m.y.y * m.z.x) * determinant
)
);
return(mat);
}
public static FMat4 NonhomogeneousInverse(FMat4 m)
{
FMat3 m3 = m;
m3.Invert();
FMat4 o = m3;
FVec4 v = m3 * m.w;
o.w.x = -v.x;
o.w.y = -v.y;
o.w.z = -v.z;
return(o);
}
public void Matrix4()
{
FMat4 fm = FMat4.FromQuaternion(FQuat.Euler(( Fix64 )30, ( Fix64 )(-20), ( Fix64 )49.342f));
Mat4 m = Mat4.FromQuaternion(Quat.Euler(30, -20, 49.342f));
FVec3 fv = new FVec3(12.5f, 9, 8);
FVec3 fv2 = new FVec3(4, 6, 9);
Vec3 v = new Vec3(12.5f, 9, 8);
Vec3 v2 = new Vec3(4, 6, 9);
fv = fm.TransformPoint(fv);
fv2 = fm.TransformVector(fv2);
v = m.TransformPoint(v);
v2 = m.TransformVector(v2);
this._output.WriteLine(fm.ToString());
this._output.WriteLine(m.ToString());
this._output.WriteLine(fv.ToString());
this._output.WriteLine(fv2.ToString());
this._output.WriteLine(v.ToString());
this._output.WriteLine(v2.ToString());
fm = FMat4.FromEuler(fv);
m = Mat4.FromEuler(v);
this._output.WriteLine(fm.ToString());
this._output.WriteLine(m.ToString());
fm = FMat4.FromScale(fv);
m = Mat4.FromScale(v);
this._output.WriteLine(fm.ToString());
this._output.WriteLine(m.ToString());
fm = FMat4.FromRotationAxis(( Fix64 )35, fv);
m = Mat4.FromRotationAxis(35, v);
this._output.WriteLine(fm.ToString());
this._output.WriteLine(m.ToString());
fm = FMat4.NonhomogeneousInverse(fm);
m = Mat4.NonhomogeneousInvert(m);
this._output.WriteLine(fm.ToString());
this._output.WriteLine(m.ToString());
fm = FMat4.FromTRS(new FVec3(4, 5, 6), FQuat.identity, FVec3.one);
m = Mat4.FromTRS(new Vec3(4, 5, 6), Quat.identity, Vec3.one);
this._output.WriteLine(fm.ToString());
this._output.WriteLine(m.ToString());
fm = FMat4.NonhomogeneousInverse(fm);
m = Mat4.NonhomogeneousInvert(m);
this._output.WriteLine(fm.ToString());
this._output.WriteLine(m.ToString());
fv = fm.TransformPoint(fv);
v = m.TransformPoint(v);
this._output.WriteLine(fv.ToString());
this._output.WriteLine(v.ToString());
}
public static FMat4 Abs(FMat4 m)
{
return(new FMat4(FVec4.Abs(m.x), FVec4.Abs(m.y), FVec4.Abs(m.z), FVec4.Abs(m.w)));
}
public Maze(FPseudoRandom random,
FVec3 scale, FVec3 offset, int row, int col,
int startIndex, int endIndex, FVec2 startPointPlace)
{
this.startPointPlace = startPointPlace;
if (this.row < ( int )this.startPointPlace.y + 3 ||
this.col < ( int )this.startPointPlace.x + 3)
{
throw new Exception("The row or col invaild");
}
this.scale = scale;
this.offset = offset;
this.row = row;
this.col = col;
this._localToWorld = FMat4.FromTRS(this.offset, FQuat.identity, this.scale);
this._worldToLocal = FMat4.NonhomogeneousInverse(this._localToWorld);
//创建二维图
Graph2D graph2 = this.CreateFullDigraph(random);
//创建格子
this.CreateTiles(graph2);
//是否随机起点和终点
this.startIndex = startIndex < 0 ? this.RandomIndexWithinBorder(random) : startIndex;
if (endIndex < 0)
{
int[] candidate = new int[5];
for (int i = 0; i < 5; i++)
{
candidate[i] = this.RandomIndexWithinBorder(random);
}
endIndex = this.GetFarthestToStartIndex(this.startIndex, candidate);
}
this.endIndex = endIndex;
//初始化起点范围
HashSet <int> walkablesHs = new HashSet <int>();
this.SetStart(graph2, this.startIndex, walkablesHs);
//创建起点和终点
int[] coord = graph2.IndexToCoord(this.startIndex);
Delaunay.DVertex startVertex = new Delaunay.DVertex(coord[0], coord[1]);
coord = graph2.IndexToCoord(this.endIndex);
Delaunay.DVertex endVertex = new Delaunay.DVertex(coord[0], coord[1]);
//创建外圆周附近的顶点
List <Delaunay.DVertex> vertices1 = this.GenVertices(24, 1f, 0.8f, 24, random);
//创建内圆周附近的顶点
List <Delaunay.DVertex> vertices2 = this.GenVertices(12, 0.2f, 0.6f, 12, random);
//合并所有顶点
List <Delaunay.DVertex> vertices = new List <Delaunay.DVertex> {
startVertex, endVertex
};
vertices.AddRange(vertices1);
vertices.AddRange(vertices2);
//点集合的三角化
List <Delaunay.DTriangle> triangles = Delaunay.Triangulate(vertices);
//从三角形集合创建图
GraphBase graph = Tools.TrianglesToGraph(triangles, random.NextFloat);
//Prim算法创建最小生成树
List <GraphEdge> edges = GraphSearcher.PrimSearch(graph, triangles[0].v0);
//每条边用A*算法生成路径
int count = edges.Count;
for (int i = 0; i < count; i++)
{
GraphEdge edge = edges[i];
Delaunay.DVertex v0 = vertices[edge.from];
Delaunay.DVertex v1 = vertices[edge.to];
int[] path = GraphSearcher.AStarSearch(graph2, graph2.CoordToIndex(( int )v0.x, ( int )v0.y),
graph2.CoordToIndex(( int )v1.x, ( int )v1.y));
this.SetPathWalkable(path, walkablesHs);
//把顶点扩展成房间
if (i == 0)
{
this.SetIndexToWalkable(graph2, ( int )v0.x, ( int )v0.y, random.Next(1, 3), random.Next(1, 3), walkablesHs);
}
this.SetIndexToWalkable(graph2, ( int )v1.x, ( int )v1.y, random.Next(1, 3), random.Next(1, 3), walkablesHs);
}
this.walkables = walkablesHs.ToArray();
}
