void PlaneSpace1(BulletSharp.Vector3 n, out BulletSharp.Vector3 p, out BulletSharp.Vector3 q)
{
if (Math.Abs(n[2]) > (Math.Sqrt(2) / 2))
{
// choose p in y-z plane
float a = n[1] * n[1] + n[2] * n[2];
float k = 1.0f / (float)Math.Sqrt(a);
p = new BulletSharp.Vector3(0, -n[2] * k, n[1] * k);
// set q = n x p
q = BulletSharp.Vector3.Cross(n, p);
}
else
{
// choose p in x-y plane
float a = n[0] * n[0] + n[1] * n[1];
float k = 1.0f / (float)Math.Sqrt(a);
p = new BulletSharp.Vector3(-n[1] * k, n[0] * k, 0);
// set q = n x p
q = BulletSharp.Vector3.Cross(n, p);
}
}
public void RenderSoftBody(SoftBody softBody)
{
Cull cullMode = device.GetRenderState <Cull>(RenderState.CullMode);
device.SetRenderState(RenderState.CullMode, Cull.None);
AlignedFaceArray faces = softBody.Faces;
int faceCount = faces.Count;
if (faceCount > 0)
{
PositionedNormal[] vectors = new PositionedNormal[faceCount * 6];
int v = 0;
int i;
for (i = 0; i < faceCount; i++)
{
NodePtrArray nodes = faces[i].N;
Node n0 = nodes[0];
Node n1 = nodes[1];
Node n2 = nodes[2];
n0.GetX(out vectors[v].Position);
n0.GetNormal(out vectors[v].Normal);
n1.GetX(out vectors[v + 1].Position);
n1.GetNormal(out vectors[v + 1].Normal);
n2.GetX(out vectors[v + 2].Position);
n2.GetNormal(out vectors[v + 2].Normal);
v += 3;
}
device.VertexFormat = VertexFormat.PositionNormal;
device.DrawUserPrimitives(PrimitiveType.TriangleList, faces.Count, vectors);
}
else
{
AlignedTetraArray tetras = softBody.Tetras;
int tetraCount = tetras.Count;
if (tetraCount > 0)
{
PositionedNormal[] vectors = new PositionedNormal[tetraCount * 12];
int v = 0;
for (int i = 0; i < tetraCount; i++)
{
NodePtrArray nodes = tetras[i].Nodes;
BulletSharp.Vector3 v0 = nodes[0].X;
BulletSharp.Vector3 v1 = nodes[1].X;
BulletSharp.Vector3 v2 = nodes[2].X;
BulletSharp.Vector3 v3 = nodes[3].X;
BulletSharp.Vector3 v10 = v1 - v0;
BulletSharp.Vector3 v02 = v0 - v2;
BulletSharp.Vector3 normal = BulletSharp.Vector3.Cross(v10, v02);
normal.Normalize();
vectors[v].Position = v0;
vectors[v].Normal = normal;
vectors[v + 1].Position = v1;
vectors[v + 1].Normal = normal;
vectors[v + 2].Position = v2;
vectors[v + 2].Normal = normal;
normal = BulletSharp.Vector3.Cross(v10, v3 - v0);
normal.Normalize();
vectors[v + 3].Position = v0;
vectors[v + 3].Normal = normal;
vectors[v + 4].Position = v1;
vectors[v + 4].Normal = normal;
vectors[v + 5].Position = v3;
vectors[v + 5].Normal = normal;
normal = BulletSharp.Vector3.Cross(v2 - v1, v3 - v1);
normal.Normalize();
vectors[v + 6].Position = v1;
vectors[v + 6].Normal = normal;
vectors[v + 7].Position = v2;
vectors[v + 7].Normal = normal;
vectors[v + 8].Position = v3;
vectors[v + 8].Normal = normal;
normal = BulletSharp.Vector3.Cross(v02, v3 - v2);
normal.Normalize();
vectors[v + 9].Position = v2;
vectors[v + 9].Normal = normal;
vectors[v + 10].Position = v0;
vectors[v + 10].Normal = normal;
vectors[v + 11].Position = v3;
vectors[v + 11].Normal = normal;
v += 12;
}
device.VertexFormat = VertexFormat.PositionNormal;
device.DrawUserPrimitives(PrimitiveType.TriangleList, tetraCount * 4, vectors);
}
else if (softBody.Links.Count > 0)
{
AlignedLinkArray links = softBody.Links;
int linkCount = links.Count;
int linkColor = System.Drawing.Color.Black.ToArgb();
device.VertexFormat = VertexFormat.Position | VertexFormat.Diffuse;
PositionColored[] linkArray = new PositionColored[linkCount * 2];
for (int i = 0; i < linkCount; i++)
{
Link link = links[i];
linkArray[i * 2].Position = link.Nodes[0].X;
linkArray[i * 2].Color = linkColor;
linkArray[i * 2 + 1].Position = link.Nodes[1].X;
linkArray[i * 2 + 1].Color = linkColor;
}
device.DrawUserPrimitives(PrimitiveType.LineList, links.Count, linkArray);
}
}
device.SetRenderState(RenderState.CullMode, cullMode);
}