public float4(float3 a, float w)
{
this.x = a.x;
this.y = a.y;
this.z = a.z;
this.w = w;
}
public float4x3(float3 r0, float3 r1, float3 r2, float3 r3)
{
M11 = r0.x; M12 = r0.y; M13 = r0.z;
M21 = r1.x; M22 = r1.y; M23 = r1.z;
M31 = r2.x; M32 = r2.y; M33 = r2.z;
M41 = r3.x; M42 = r3.y; M43 = r3.z;
}
public static void AngleMatrix (float3 angles, ref float4x4 matrix)
{
float angle;
float sr, sp, sy, cr, cp, cy;
angle = angles[2];
sy = (float)Math.Sin(angle);
cy = (float)Math.Cos(angle);
angle = angles[1];
sp = (float)Math.Sin(angle);
cp = (float)Math.Cos(angle);
angle = angles[0];
sr = (float)Math.Sin(angle);
cr = (float)Math.Cos(angle);
// matrix = (Z * Y) * X
//matrix[0,0] = cp * cy;
//matrix[1][0] = cp * sy;
//matrix[2][0] = -sp;
//matrix[0][1] = sr * sp * cy + cr * -sy;
//matrix[1][1] = sr * sp * sy + cr * cy;
//matrix[2][1] = sr * cp;
//matrix[0][2] = (cr * sp * cy + -sr * -sy);
//matrix[1][2] = (cr * sp * sy + -sr * cy);
//matrix[2][2] = cr * cp;
//matrix[0][3] = 0.0;
//matrix[1][3] = 0.0;
//matrix[2][3] = 0.0;
}
public float4x3(float3x3 R, float3 t)
{
M11 = R.M11; M12 = R.M12; M13 = R.M13;
M21 = R.M21; M22 = R.M22; M23 = R.M23;
M31 = R.M31; M32 = R.M32; M33 = R.M33;
M41 = t.x; M42 = t.y; M43 = t.z;
}
public ParticleSystem(int numParticles, Real step)
{
fNumParticles = numParticles;
fPosition = new float3[numParticles];
fVelocity = new float3[numParticles];
fAcceleration = new float3[numParticles];
fMass = new float[numParticles];
fMassInverse = new float[numParticles];
SetStep(step);
// Setup variables used by the Runge-Kutta fourth-order solver
m_akPTmp = new float3[numParticles];
m_akDPTmp1 = new float3[numParticles];
m_akDPTmp2 = new float3[numParticles];
m_akDPTmp3 = new float3[numParticles];
m_akDPTmp4 = new float3[numParticles];
m_akVTmp = new float3[numParticles];
m_akDVTmp1 = new float3[numParticles];
m_akDVTmp2 = new float3[numParticles];
m_akDVTmp3 = new float3[numParticles];
m_akDVTmp4 = new float3[numParticles];
}
public static void VectorRotate(float3 in1, float4x4 in2, ref float3 vout)
{
//vout[0] = in1 * in2[0];// DotProduct(in1, in2[0]);
//vout[1] = in1 * in2[1];// DotProduct(in1, in2[1]);
//vout[2] = in1 * in2[2];// DotProduct(in1, in2[2]);
}
public float4x4(float3x3 R, float3 t)
{
M11 = R.M11; M12 = R.M12; M13 = R.M13; M14 = 0;
M21 = R.M21; M22 = R.M22; M23 = R.M23; M24 = 0;
M31 = R.M31; M32 = R.M32; M33 = R.M33; M34 = 0;
M41 = t.x; M42 = t.y; M43 = t.z; M44 = 1;
}
protected void calculateNormals()
{
float3 v1 = new float3();
float3 v2 = new float3();
float3 v;
int p1, p2, p3;
int i;
for (i = 0; i < numPart; i++)
normals[0] = float3.Zero;
for (i = 0; i < triangles.Length; i++)
{
p1 = triangles[i].p1;
p2 = triangles[i].p2;
p3 = triangles[i].p3;
v1 = positions[p2] - positions[p1];
v2 = positions[p3] - positions[p1];
v = float3.Cross(v1, v2);
normals[p1] += v;
normals[p2] += v;
normals[p3] += v;
}
// Normalize the normals into unit vectors
for (i = 0; i < numPart; i++)
normals[i] = normals[i].Normalize;
}
public static float3 Cross(float3 u, float3 v)
{
float3 n = new float3();
n.x = u.y * v.z - u.z * v.y;
n.y = u.z * v.x - u.x * v.z;
n.z = u.x * v.y - u.y * v.x;
return n;
}
public SpeakerCube(GraphicsInterface gi, GLTexture faceTexture)
:base(gi, faceTexture, 4, 4.0f, 4.0f, 10, 10, 1.0f)
{
Translation = new float3(0, 4.0f, 0.0);
fSpeakerSeparation = 0.30f;
// Sections displaying current speaker
fSpeakerSection0 = new CylinderSection(GI, 4.0f, 4.0f, 45, 10, 10);
fSpeakerSection90 = new CylinderSection(GI, 4.0f, 4.0f, 45, 10, 10);
fSpeakerSection180 = new CylinderSection(GI, 4.0f, 4.0f, 45, 10, 10);
fSpeakerSection270 = new CylinderSection(GI, 4.0f, 4.0f, 45, 10, 10);
}
public static void AddPointToBounds (float3 v, ref float3 mins, ref float3 maxs)
{
int i;
float val;
for (i = 0; i < 3; i++)
{
val = v[i];
if (val < mins[i])
mins[i] = val;
if (val > maxs[i])
maxs[i] = val;
}
}
public GLSpear(float3 vec, ColorRGBA shaftColor)
{
fShaftColor = shaftColor;
fVector = vec;
fSpearLength = fVector.Length;
fDisk = new GLUDisk(fAxisRadius, fAxisRadius, 10, 1);
fDisk.DrawingStyle = QuadricDrawStyle.Fill;
fDisk.NormalType = QuadricNormalType.Smooth;
fDisk.Orientation = QuadricOrientation.Outside;
fDisk.UsesTexture = 0;
fAxisShaft = new GLUCylinder(fAxisRadius, fSpearLength, 10, 1);
fAxisShaft.NormalType = QuadricNormalType.Smooth;
fAxisShaft.Orientation = QuadricOrientation.Outside;
fAxisShaft.UsesTexture = 0;
fArrowHead = new GLUCone(fArrowRadius, fArrowHeight, 10, 1);
fArrowHead.NormalType = QuadricNormalType.Smooth;
fArrowHead.Orientation = QuadricOrientation.Outside;
fArrowHead.UsesTexture = 0;
}
public void AddVertex(float3 vec)
{
GI.Vertex(vec);
}
public static void VectorTransform(float3 in1, float4x4 in2, ref float3 vout)
{
//vout[0] = in1 * in2[0] + in2[0][3];
//vout[1] = in1 * in2[1] + in2[1][3];
//vout[2] = in1 * in2[2] + in2[2][3];
}
public static void VectorITransform(float3 in1, float4x4 in2, ref float3 vout)
{
float3 tmp = new float3();
tmp[0] = in1[0] - in2[0][3];
tmp[1] = in1[1] - in2[1][3];
tmp[2] = in1[2] - in2[2][3];
VectorIRotate(tmp, in2, ref vout);
}
public static void VectorIRotate(float3 in1, float4x4 in2, ref float3 vout)
{
vout[0] = in1[0] * in2[0][0] + in1[1] * in2[1][0] + in1[2] * in2[2][0];
vout[1] = in1[0] * in2[0][1] + in1[1] * in2[1][1] + in1[2] * in2[2][1];
vout[2] = in1[0] * in2[0][2] + in1[1] * in2[1][2] + in1[2] * in2[2][2];
}
/// <summary>
/// Rotation by angle theta about unit axis u
/// </summary>
public static float3x3 Rotate(float theta, float3 u)
{
if (theta == 0f) return Identity;
float3 w = new float3(theta * u.x, theta * u.y, theta * u.z);
float3x3 wx = w.CRS;
return Identity + (((float)Math.Sin(theta)) / theta) * wx + ((1 - ((float)Math.Cos(theta))) / (theta * theta)) * (wx * wx);
}
public static float3x3 Cross(float3 v)
{
return new float3x3(0, -v.z, v.y, v.z, 0, -v.x, -v.y, v.x, 0);
}
public GLVertex(float3 position, ColorRGBA color)
{
fPosition = position;
fColor = color;
}
protected virtual void verlet()
{
// | positions = 2*positions - oldPositions + forces * timeStep * timeStep
// | oldPositions = positions
float3[] newPositions = new float3[numPart];
float timeStep_2 = timeStep * timeStep;
for (int i = 0; i < numPart; i++)
{
for (int j = 0; j < 3; j++)
{
newPositions[i][j] = 1.96f * positions[i][j] - 0.96f * oldPositions[i][j] + forces[i][j] * timeStep_2;
}
}
vectorCopy(oldPositions, positions); //oldPositions = positions
vectorCopy(positions, newPositions); //positions = newPositions
}
protected void vectorCopy(float3[] dst, float3[] src)
{
for (int i = 0; i < numPart; i++)
dst[i] = src[i];
}
public void Material(GLFace face, MaterialParameter pname, float3 parameters)
{
gl.glMaterialfv((int)face, (int)pname, (float[])parameters);
// Don't check for error, because this is called between
// glBegin()/glEnd()
//CheckException();
}
void ReadJoints(BinaryReader fp)
{
int i, j;
ushort numJoints;
float3 angles = new float3();
numJoints = fp.ReadUInt16();
m_joints.Capacity = numJoints;
for (i = 0; i < numJoints; i++)
{
m_joints.Add(new MS3DJoint());
m_joints[i].flags = fp.ReadByte();
m_joints[i].name = new string(fp.ReadChars(32));
m_joints[i].parentName = new string(fp.ReadChars(32));
angles = ReadFloat3(fp); // Euler Angles, need to convert to quaternion
m_joints[i].fInitialRotation = Quaternion.FromEulerAngles(angles);
m_joints[i].fInitialPosition = ReadFloat3(fp);
ushort numKeyFramesRot;
numKeyFramesRot = fp.ReadUInt16();
m_joints[i].rotationKeys.Capacity = numKeyFramesRot;
ushort numKeyFramesPos;
numKeyFramesPos = fp.ReadUInt16();
m_joints[i].fPositionKeyframes.Capacity = numKeyFramesPos;
// the frame time is in seconds, so multiply it by the animation fps, to get the frame number
// rotation channel
for (j = 0; j < numKeyFramesRot; j++)
{
m_joints[i].rotationKeys.Add(new MS3DRotationFrame());
m_joints[i].rotationKeys[j].time = fp.ReadSingle();
m_joints[i].rotationKeys[j].time *= m_animationFps;
angles = ReadFloat3(fp);
m_joints[i].rotationKeys[j].mRotation = Quaternion.FromEulerAngles(angles);
}
// translation channel
for (j = 0; j < numKeyFramesPos; j++)
{
m_joints[i].fPositionKeyframes.Add(new MS3DTranslationFrame());
m_joints[i].fPositionKeyframes[j].fTime = fp.ReadSingle();
m_joints[i].fPositionKeyframes[j].fTime *= m_animationFps;
m_joints[i].fPositionKeyframes[j].fTranslation = ReadFloat3(fp);
}
}
InitializeJoints();
}
public void Vertex(float3 vec)
{
gl.glVertex3f(vec.x, vec.y, vec.z);
// Don't check for error, because this is called between
// glBegin()/glEnd()
//CheckException();
}
void CalculateDimensions()
{
// Calculate dimensions
float3 mins = new float3();
float3 maxs = new float3();
MS3DMath.ClearBounds(ref mins, ref maxs);
foreach (MS3DVertex vertex in Vertices)
{
MS3DMath.AddPointToBounds(vertex.vertex, ref mins, ref maxs);
}
dim[0] = maxs[0] - mins[0];
dim[1] = maxs[1] - mins[1];
dim[2] = maxs[2] - mins[2];
center[0] = mins[0] + dim[0] / 2.0f;
center[1] = mins[1] + dim[1] / 2.0f;
center[2] = mins[2] + dim[2] / 2.0f;
radius = dim[0];
if (dim[1] > radius)
radius = dim[1];
if (dim[2] > radius)
radius = dim[2];
radius *= 1.41f;
}
public float3x3(float3 r0, float3 r1, float3 r2)
{
M11 = r0.x; M12 = r0.y; M13 = r0.z;
M21 = r1.x; M22 = r1.y; M23 = r1.z;
M31 = r2.x; M32 = r2.y; M33 = r2.z;
}
protected virtual void satisfyConstraints()
{
float3 d = new float3();
float dLength;
float diff;
int p1, p2;
for (int n = 0; n < 2; n++)
{
for (int i = 0; i < constraints.Length; i++)
{
p1 = constraints[i].p1;
p2 = constraints[i].p2;
d = positions[p2] - positions[p1];
dLength = (float)Math.Sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]);
diff = (dLength - constraints[i].dist) / dLength;
positions[p1][0] += 0.5f * diff * d[0];
positions[p1][1] += 0.5f * diff * d[1];
positions[p1][2] += 0.5f * diff * d[2];
positions[p2][0] -= 0.5f * diff * d[0];
positions[p2][1] -= 0.5f * diff * d[1];
positions[p2][2] -= 0.5f * diff * d[2];
}
}
}
public static void ClearBounds (ref float3 mins, ref float3 maxs)
{
mins[0] = mins[1] = mins[2] = 99999;
maxs[0] = maxs[1] = maxs[2] = -99999;
}
public void VertexPointer(float3[] pointer)
{
gl.glVertexPointer(3, (int)VertexPointerType.Float, 0, pointer);
CheckException();
}
float3 ReadFloat3(BinaryReader reader)
{
float3 newFloat3 = new float3();
newFloat3.x = reader.ReadSingle();
newFloat3.y = reader.ReadSingle();
newFloat3.z = reader.ReadSingle();
return newFloat3;
}
