public void addTriangle(float3 p1, float3 p2, float3 p3, float4 clr)
{
addPoint(p1, clr);
addPoint(p2, clr);
addPoint(p3, clr);
}
public void addLine(float3 p1, float3 p2, float4 clr)
{
addPoint(p1, clr);
addPoint(p2, clr);
}
public void addPoint(float3 pt, float4 clr)
{
addPt(pt);
addClr(clr);
indices_.Add((uint)indices_.Count());
}
public void addNormal(float3 normal)
{
normals_.Add(normal.x);
normals_.Add(normal.y);
normals_.Add(normal.z);
}
public void addPt(float3 pt)
{
vertices_.Add(pt.x);
vertices_.Add(pt.y);
vertices_.Add(pt.z);
}
public void addCylinder(float3 startPosition, float3 endPosition, float4 clr)
{
/////////////////////////////
/// Compute Rotation Matrix
/////////////////////////////
const float PI = 3.1415926f;
float m_radius = 0.010f;
float3 dir = endPosition.sub(startPosition);
float m_height = dir.norm();
float x = 0.0f, y = 0.0f, z = 0.0f;
dir.divide(m_height);
float3 yAxis = new float3(0, 1, 0);
float3 v = dir.cross(yAxis);;
Matrix4x4 rotation;
float sinTheta = v.norm();
if (sinTheta < 0.00001f)
{
rotation = Matrix4x4.Identity;
}
else
{
float cosTheta = dir.dot(yAxis);
float scale = (1.0f - cosTheta) / (1.0f - (cosTheta * cosTheta));
Matrix4x4 vx = new Matrix4x4(0, v.z, -v.y, 0,
-v.z, 0, v.x, 0,
v.y, -v.x, 0, 0,
0, 0, 0, 1.0f);
Matrix4x4 vx2 = vx * vx;
Matrix4x4 vx2Scaled = vx2 * scale;
rotation = Matrix4x4.Identity;
rotation = rotation + vx;
rotation = rotation + vx2Scaled;
}
/////////////////////////////
/// Create Cylinder
/////////////////////////////
Matrix3x3 rotationMatrix = new Matrix3x3();
float[] data = new float[9] {
rotation.M11, rotation.M12, rotation.M13, rotation.M21, rotation.M22, rotation.M23, rotation.M31, rotation.M32, rotation.M33
};
rotationMatrix.m = data;
float3 v1;
float3 v2;
float3 v3;
float3 v4;
float3 normal;
float resolution = 0.1f;
for (double i = 0; i <= 2 * PI - 1; i += resolution)
{
v1 = rotationMatrix.multiply(new float3(m_radius * (float)Math.Cos(i), 0, m_radius * (float)Math.Sin(i))).add(startPosition);
v2 = rotationMatrix.multiply(new float3(m_radius * (float)Math.Cos(i), m_height, m_radius * (float)Math.Sin(i))).add(startPosition);
v4 = rotationMatrix.multiply(new float3(m_radius * (float)Math.Cos(i + 1), m_height, m_radius * (float)Math.Sin(i + 1))).add(startPosition);
v3 = rotationMatrix.multiply(new float3(m_radius * (float)Math.Cos(i + 1), 0, m_radius * (float)Math.Sin(i + 1))).add(startPosition);
float3 a = v2.sub(v1);
float3 b = v3.sub(v1);
normal = a.cross(b);
normal.divide(normal.norm());
addPoint(v1, clr);
addPoint(v2, clr);
addPoint(v4, clr);
addPoint(v3, clr);
addNormal(normal);
addNormal(normal);
addNormal(normal);
addNormal(normal);
}
}