/** @brief updates the stored jacobian for the current values in the pivots*/
void InvKinematic::ConstructJacobian() {
// ds_i / dtheta_j = v_j x (s_i - p_j)
//
// (u2*v3 - u3*v2)
// u x v = (u3*v1 - u1*v3)
// (u1*v2 - u2*v1)
const int end = pivot_count_ - 1; // don't include the end effector
double unity_z[3] = {0.0, 0.0, 1.0};
double v_j[3];
HMatrix H_0_s = HMatrix(
pivots_[pivot_count_ - 1]->GetRelativeHMatrixArray());
HMatrix H_0_pj;
// printf(" transposed jacobian:\n");
for (int j = 0; j < end; ++j) {
H_0_pj = HMatrix(pivots_[j]->GetRelativeHMatrixArray());
H_0_pj.RotateVector(unity_z, v_j);
const double v_diff[3] = {
H_0_s.GetX() - H_0_pj.GetX(),
H_0_s.GetY() - H_0_pj.GetY(),
H_0_s.GetZ() - H_0_pj.GetZ()};
CrossProduct3(v_j, v_diff, &jacobianT_[j*3]);
// printf(" %.4f, %.4f, %.4f\n", jacobianT_[j*3], jacobianT_[j*3+1],
// jacobianT_[j*3+2]);
}
}
//----------------------------------------------------------------------------
void PolyhedronDistance::CreateScene ()
{
// ** layout of scene graph **
// scene
// tetra[4]
// plane
// line[2]
// Create the objects.
mScene = new0 Node();
mWireState = new0 WireState();
mRenderer->SetOverrideWireState(mWireState);
TriMesh* plane = CreatePlane();
int i;
for (i = 0; i < 2; ++i)
{
// Build the display tetrahedra.
float size = 0.3f + 0.2f*(i + 1);
if (i == 0)
{
mEdgeLength = size;
}
mTetras[i] = CreateTetra(size, false);
mSegments[i] = CreateSegment();
// Build the point tetrahedra.
mSmall = 0.02f;
mTetras[i + 2] = CreateTetra(mSmall, true);
}
// Tetrahedra faces.
mFaces = new1<Tuple<3,int> >(4);
mFaces[0][0] = 1; mFaces[0][1] = 2; mFaces[0][2] = 0;
mFaces[1][0] = 0; mFaces[1][1] = 3; mFaces[1][2] = 2;
mFaces[2][0] = 0; mFaces[2][1] = 1; mFaces[2][2] = 3;
mFaces[3][0] = 1; mFaces[3][1] = 2; mFaces[3][2] = 3;
// Transform the tetrahedra.
mTetras[0]->LocalTransform.SetRotate(HMatrix(AVector::UNIT_Z, 1.1f));
mTetras[0]->LocalTransform.SetTranslate(APoint(-0.25f, 0.1f, 0.3f));
mTetras[1]->LocalTransform.SetRotate(HMatrix(AVector::UNIT_Z, 0.3f));
mTetras[1]->LocalTransform.SetTranslate(APoint(0.25f, 0.4f, 0.5f));
// Set parent-child links.
mScene->AttachChild(plane);
for (i = 0; i < 2; ++i)
{
mScene->AttachChild(mTetras[i]);
mScene->AttachChild(mSegments[i]);
mScene->AttachChild(mTetras[i + 2]);
}
}
//----------------------------------------------------------------------------
void PhysicsModule::GetData (APoint& center, HMatrix& incrRot) const
{
// Position is a point exactly on the hill.
APoint position;
position[0] = (float)(A1*mState[0]);
position[1] = (float)(A2*mState[2]);
position[2] = (float)(A3 - mState[0]*mState[0] - mState[2]*mState[2]);
// Lift this point off the hill in the normal direction by the radius of
// the ball so that the ball just touches the hill. The hill is
// implicitly specified by F(x,y,z) = z - [a3 - (x/a1)^2 - (y/a2)^2]
// where (x,y,z) is the position on the hill. The gradient of F is a
// normal vector, Grad(F) = (2*x/a1^2,2*y/a2^2,1).
AVector normal;
normal[0] = 2.0f*position[0]/(float)mAux[0];
normal[1] = 2.0f*position[1]/(float)mAux[1];
normal[2] = 1.0f;
normal.Normalize();
center = position + ((float)Radius)*normal;
// Let the ball rotate as it rolls down hill. The axis of rotation is
// the perpendicular to hill normal and ball velocity. The angle of
// rotation from the last position is A = speed*deltaTime/radius.
AVector velocity;
velocity[0] = (float)(A1*mState[1]);
velocity[1] = (float)(A1*mState[3]);
velocity[2] = -2.0f*(velocity[0]*(float)mState[0] +
velocity[1]*(float)mState[2]);
float speed = velocity.Normalize();
float angle = speed*((float)mDeltaTime)/((float)Radius);
AVector axis = normal.UnitCross(velocity);
incrRot = HMatrix(axis, angle);
}
//----------------------------------------------------------------------------
void PlanarReflections::CreateScene ()
{
mScene = new0 Node();
mWireState = new0 WireState();
mRenderer->SetOverrideWireState(mWireState);
LoadBiped();
CreatePlanes();
CreatePlanarReflection();
mScene->LocalTransform.SetRotate(HMatrix(AVector::UNIT_Z, 0.75f));
}
//----------------------------------------------------------------------------
void SimplePendulumFriction::PhysicsTick ()
{
mModule.Update();
// Update the pendulum mechanism. The pendulum rod is attached at
// (x,y,z) = (0,0,16). The update here has the 16 hard-coded.
mPendulum->LocalTransform.SetRotate(HMatrix(AVector::UNIT_X,
(float)mModule.GetTheta()));
mPendulum->Update();
}
//----------------------------------------------------------------------------
void RoughPlaneSolidBox::CreateScene ()
{
mScene = new0 Node();
mWireState = new0 WireState();
mRenderer->SetOverrideWireState(mWireState);
mScene->AttachChild(CreateGround());
mScene->AttachChild(CreateRamp());
mScene->AttachChild(CreateBox());
mScene->LocalTransform.SetRotate(HMatrix(AVector::UNIT_Z, 0.661917f));
}
//----------------------------------------------------------------------------
void Castle::CreateLights ()
{
mDLight = new0 Light(Light::LT_DIRECTIONAL);
mDLight->Ambient = Float4(1.0f, 1.0f, 1.0f, 1.0f);
mDLight->Diffuse = Float4(1.0f, 1.0f, 1.0f, 1.0f);
mDLight->Specular = Float4(1.0f, 1.0f, 1.0f, 1.0f);
LightNode* lightNode = new0 LightNode(mDLight);
lightNode->LocalTransform.SetTranslate(APoint(1628.448730f, -51.877197f,
0.0f));
lightNode->LocalTransform.SetRotate(HMatrix(AVector(-1.0f, 0.0f, 0.0f),
Mathf::HALF_PI));
mDLightRoot = new0 Node();
mDLightRoot->LocalTransform.SetTranslate(APoint(-1824.998657f,
-1531.269775f, 3886.592773f));
mDLightRoot->LocalTransform.SetRotate(HMatrix(AVector(-0.494124f,
0.325880f, 0.806005f), 1.371538f));
mDLightRoot->AttachChild(lightNode);
mDLightRoot->Update();
}
//----------------------------------------------------------------------------
void RoughPlaneSolidBox::MoveBox ()
{
float x = (float)mModule.GetX();
float w = (float)mModule.GetW();
float xExt = (float)mModule.XLocExt;
float yExt = (float)mModule.YLocExt;
float zExt = (float)mModule.ZLocExt;
float sinPhi = (float)mModule.SinAngle;
float cosPhi = (float)mModule.CosAngle;
float theta = (float)mModule.GetTheta();
float sinTheta = Mathf::Sin(theta);
float cosTheta = Mathf::Cos(theta);
// Compute the box center.
APoint center(x, w*cosPhi - zExt*sinPhi, w*sinPhi + zExt*cosPhi);
// Compute the box orientation.
AVector axis0(cosTheta, -sinTheta*cosPhi, -sinTheta*sinPhi);
AVector axis1(sinTheta, +cosTheta*cosPhi, +cosTheta*sinPhi);
AVector axis2(0.0f, -sinPhi, cosPhi);
// Keep the box from sliding below the ground.
float zRadius =
xExt*Mathf::FAbs(axis0.Z()) +
yExt*Mathf::FAbs(axis1.Z()) +
zExt*Mathf::FAbs(axis2.Z());
if (center.Z() >= zRadius)
{
// Update the box.
mBox->LocalTransform.SetTranslate(center);
mBox->LocalTransform.SetRotate(HMatrix(axis0, axis1, axis2,
APoint::ORIGIN, true));
mBox->Update();
}
else
{
mDoUpdate = false;
}
}
//----------------------------------------------------------------------------
void WaterDropFormation::CreateWall ()
{
VertexFormat* vformat = VertexFormat::Create(2,
VertexFormat::AU_POSITION, VertexFormat::AT_FLOAT3, 0,
VertexFormat::AU_TEXCOORD, VertexFormat::AT_FLOAT2, 0);
StandardMesh sm(vformat);
Transform transform;
transform.SetTranslate(APoint(-8.0f, 0.0f, 8.0f));
transform.SetRotate(HMatrix(AVector::UNIT_Y, AVector::UNIT_Z,
AVector::UNIT_X, APoint::ORIGIN, true));
sm.SetTransform(transform);
mWall = sm.Rectangle(2, 2, 16.0f, 8.0f);
std::string path = Environment::GetPathR("Stone.wmtf");
Texture2D* texture = Texture2D::LoadWMTF(path);
mWall->SetEffectInstance(Texture2DEffect::CreateUniqueInstance(texture,
Shader::SF_LINEAR, Shader::SC_CLAMP_EDGE, Shader::SC_CLAMP_EDGE));
mTrnNode->AttachChild(mWall);
}
//----------------------------------------------------------------------------
void IntersectInfiniteCylinders::CreateScene ()
{
mScene = new0 Node();
mWireState = new0 WireState();
mRenderer->SetOverrideWireState(mWireState);
VertexFormat* vformat = VertexFormat::Create(2,
VertexFormat::AU_POSITION, VertexFormat::AT_FLOAT3, 0,
VertexFormat::AU_COLOR, VertexFormat::AT_FLOAT3, 0);
int vstride = vformat->GetStride();
StandardMesh sm(vformat);
VertexBufferAccessor vba;
int i;
// Create the canonical cylinder.
mCylinder0 = sm.Cylinder(32, 128, mRadius0, mHeight, true);
mScene->AttachChild(mCylinder0);
mVisible.Insert(mCylinder0);
vba.ApplyTo(mCylinder0);
for (i = 0; i < vba.GetNumVertices(); ++i)
{
vba.Color<Float3>(0, i) = Float3(0.5f, 0.0f, 0.0f);
}
mCylinder0->SetEffectInstance(VertexColor3Effect::CreateUniqueInstance());
// Create the other cylinder.
mCylinder1 = sm.Cylinder(32, 128, mRadius1, mHeight, true);
mScene->AttachChild(mCylinder1);
mVisible.Insert(mCylinder1);
vba.ApplyTo(mCylinder1);
for (i = 0; i < vba.GetNumVertices(); ++i)
{
vba.Color<Float3>(0, i) = Float3(0.0f, 0.0f, 0.5f);
}
mCylinder1->SetEffectInstance(VertexColor3Effect::CreateUniqueInstance());
mCylinder1->LocalTransform.SetRotate(HMatrix(AVector::UNIT_X, -mAngle));
mCylinder1->LocalTransform.SetTranslate(APoint(mC0, 0.0f, 0.0f));
// Create the intersection curve.
const float minTheta = 2.0f*Mathf::PI/3.0f;
const float maxTheta = 4.0f*Mathf::PI/3.0f;
float theta, cs, sn, t, tmp, discr;
VertexBuffer* vbuffer = new0 VertexBuffer(1024, vstride);
mCurve0 = new0 Polysegment(vformat, vbuffer, true);
mScene->AttachChild(mCurve0);
vba.ApplyTo(mCurve0);
int numPoints = vba.GetNumVertices();
float multiplier = (maxTheta - minTheta)/(float)(numPoints - 1);
for (i = 0; i < numPoints; ++i)
{
theta = minTheta + multiplier*i;
cs = Mathf::Cos(theta);
sn = Mathf::Sin(theta);
tmp = mC0 + mRadius1*cs;
discr = Mathf::FAbs(mRadius0*mRadius0 - tmp*tmp);
t = (-mRadius1*mW2*sn - Mathf::Sqrt(discr))/mW1;
Float3& position = vba.Position<Float3>(i);
position[0] = mC0 + mRadius1*cs;
position[1] = +mRadius1*sn*mW2 + t*mW1;
position[2] = -mRadius1*sn*mW1 + t*mW2;
vba.Color<Float3>(0, i) = Float3(0.0f, 0.5f, 0.0f);
}
mCurve0->SetEffectInstance(VertexColor3Effect::CreateUniqueInstance());
mVisible.Insert(mCurve0);
vbuffer = new0 VertexBuffer(1024, vstride);
mCurve1 = new0 Polysegment(vformat, vbuffer, true);
mScene->AttachChild(mCurve1);
vba.ApplyTo(mCurve1);
numPoints = vba.GetNumVertices();
multiplier = (maxTheta - minTheta)/(float)(numPoints - 1);
for (i = 0; i < numPoints; ++i)
{
theta = minTheta + multiplier*i;
cs = Mathf::Cos(theta);
sn = Mathf::Sin(theta);
tmp = mC0 + mRadius1*cs;
discr = Mathf::FAbs(mRadius0*mRadius0 - tmp*tmp);
t = (-mRadius1*mW2*sn + Mathf::Sqrt(discr))/mW1;
Float3& position = vba.Position<Float3>(i);
position[0] = mC0 + mRadius1*cs;
position[1] = +mRadius1*sn*mW2 + t*mW1;
position[2] = -mRadius1*sn*mW1 + t*mW2;
vba.Color<Float3>(0, i) = Float3(0.0f, 0.5f, 0.0f);
}
mCurve1->SetEffectInstance(VertexColor3Effect::CreateUniqueInstance());
mVisible.Insert(mCurve1);
}
//----------------------------------------------------------------------------
void BouncingBall::PhysicsTick ()
{
// Update the ball.
mBall->DoSimulationStep(mSimTime);
mRenderer->Update(mBall->GetMesh()->GetVertexBuffer());
// Get the ball parameters.
float period = mBall->GetPeriod();
float tMin = mBall->GetMinActive();
float tMax = mBall->GetMaxActive();
// Translate the ball.
const float yMax = 2.5f, zMax = 0.75f;
float yTrn, zTrn, ratio, amp;
float time = fmodf(mSimTime, 2.0f*period);
if (time < tMin)
{
ratio = time/tMin;
yTrn = yMax*ratio;
zTrn = zMax*(1.0f - ratio*ratio);
}
else if (time < tMax)
{
yTrn = yMax;
amp = mBall->GetAmplitude(time);
if (amp <= 0.999f)
{
zTrn = -(1.0f - Mathf::Sqrt(1.0f - amp + amp*amp))/(1.0f - amp);
}
else
{
zTrn = -0.5f;
}
}
else if (time < period + tMin)
{
yTrn = -yMax*(time - period)/tMin;
zTrn = zMax*(time - tMax)*(period + tMin - time) /
(tMin*(period - tMax));
}
else if (time < period + tMax)
{
yTrn = -yMax;
amp = mBall->GetAmplitude(time - period);
if (amp <= 0.999f)
{
zTrn = -(1.0f - Mathf::Sqrt(1.0f - amp + amp*amp))/(1.0f - amp);
}
else
{
zTrn = -0.5f;
}
}
else
{
yTrn = yMax*(time - 2.0f*period)/(period - tMax);
zTrn = zMax*(time - (period + tMax))*(2.0f*period + tMin - time) /
(tMin*(period - tMax));
}
mBallNode->LocalTransform.SetTranslate(APoint(0.0f, yTrn, zTrn));
// Rotate the ball.
float angle = Mathf::HALF_PI + 0.5f*yTrn*Mathf::PI/yMax;
mBallNode->LocalTransform.SetRotate(HMatrix(AVector::UNIT_Z, angle));
// Update the scene graph.
mBallNode->Update();
// Next simulation time.
mSimTime += mSimDelta;
}
//----------------------------------------------------------------------------
void GlossMaps::CreateScene ()
{
mScene = new0 Node();
mTrnNode = new0 Node();
mScene->AttachChild(mTrnNode);
// Create vertex and index buffers to be shared by two meshes.
VertexFormat* vformat = VertexFormat::Create(3,
VertexFormat::AU_POSITION, VertexFormat::AT_FLOAT3, 0,
VertexFormat::AU_NORMAL, VertexFormat::AT_FLOAT3, 0,
VertexFormat::AU_TEXCOORD, VertexFormat::AT_FLOAT2, 0);
int vstride = vformat->GetStride();
VertexBuffer* vbuffer = new0 VertexBuffer(4, vstride);
VertexBufferAccessor vba(vformat, vbuffer);
Float3 yVector(0.0f, 1.0f, 0.0f);
vba.Position<Float3>(0) = Float3(-0.5f, 0.0f, -0.5f);
vba.Position<Float3>(1) = Float3(-0.5f, 0.0f, 0.5f);
vba.Position<Float3>(2) = Float3( 0.5f, 0.0f, 0.5f);
vba.Position<Float3>(3) = Float3( 0.5f, 0.0f, -0.5f);
vba.Normal<Float3>(0) = yVector;
vba.Normal<Float3>(1) = yVector;
vba.Normal<Float3>(2) = yVector;
vba.Normal<Float3>(3) = yVector;
vba.TCoord<Float2>(0, 0) = Float2(1.0f, 0.0f);
vba.TCoord<Float2>(0, 1) = Float2(1.0f, 1.0f);
vba.TCoord<Float2>(0, 2) = Float2(0.0f, 1.0f);
vba.TCoord<Float2>(0, 3) = Float2(0.0f, 0.0f);
IndexBuffer* ibuffer = new0 IndexBuffer(6, sizeof(int));
int* indices = (int*)ibuffer->GetData();
indices[0] = 0; indices[1] = 1; indices[2] = 3;
indices[3] = 3; indices[4] = 1; indices[5] = 2;
// The light and material are used by both the gloss and non-gloss
// objects.
Light* light = new0 Light(Light::LT_DIRECTIONAL);
light->Ambient = Float4(0.1f, 0.1f, 0.1f, 1.0f);
light->Diffuse = Float4(0.6f, 0.6f, 0.6f, 1.0f);
light->Specular = Float4(1.0f, 1.0f, 1.0f, 1.0f);
light->DVector = AVector(0.0f, -1.0f, 0.0f);
Material* material = new0 Material();
material->Ambient = Float4(0.2f, 0.2f, 0.2f, 1.0f);
material->Diffuse = Float4(0.7f, 0.7f, 0.7f, 1.0f);
material->Specular = Float4(1.0f, 1.0f, 1.0f, 25.0f);
// Create a non-gloss-mapped square.
TriMesh* squareNoGloss = new0 TriMesh(vformat, vbuffer, ibuffer);
squareNoGloss->LocalTransform.SetRotate(HMatrix(AVector::UNIT_X,
-0.25f*Mathf::PI));
squareNoGloss->LocalTransform.SetTranslate(APoint(1.0f, -1.0f, 0.0f));
squareNoGloss->SetEffectInstance(
LightDirPerVerEffect::CreateUniqueInstance(light, material));
mTrnNode->AttachChild(squareNoGloss);
// Create a gloss-mapped square.
TriMesh* squareGloss = new0 TriMesh(vformat, vbuffer, ibuffer);
squareGloss->LocalTransform.SetRotate(HMatrix(AVector::UNIT_X,
-0.25f*Mathf::PI));
squareGloss->LocalTransform.SetTranslate(APoint(-1.0f, -1.0f, 0.0f));
mTrnNode->AttachChild(squareGloss);
std::string effectFile = Environment::GetPathR("GlossMap.wmfx");
GlossMapEffect* effect = new0 GlossMapEffect(effectFile);
std::string baseName = Environment::GetPathR("Magic.wmtf");
Texture2D* baseTexture = Texture2D::LoadWMTF(baseName);
squareGloss->SetEffectInstance(effect->CreateInstance(baseTexture,
light, material));
}
//----------------------------------------------------------------------------
void SkinnedBiped::CreateScene ()
{
// Allow for toggle of wireframe.
mWireState = new0 WireState();
mRenderer->SetOverrideWireState(mWireState);
// The biped has materials assigned to its triangle meshes, so they need
// lighting.
mLight = new0 Light(Light::LT_DIRECTIONAL);
mLight->Ambient = Float4(0.5f, 0.5f, 0.5f, 1.0f);
mLight->Diffuse = mLight->Ambient;
mLight->Specular = Float4(0.0f, 0.0f, 0.0f, 1.0f);
mLight->Constant = 0.0f;
mLight->Linear = 0.0f;
mLight->Quadratic = 0.0f;
mLight->Intensity = 1.0f;
mLight->DVector = mCamera->GetDVector();
Node* biped = GetNode("Biped");
Node* pelvis = GetNode("Pelvis");
Node* spine = GetNode("Spine");
Node* spine1 = GetNode("Spine1");
Node* spine2 = GetNode("Spine2");
Node* spine3 = GetNode("Spine3");
Node* neck = GetNode("Neck");
Node* head = GetNode("Head");
Node* leftClavicle = GetNode("L_Clavicle");
Node* leftUpperArm = GetNode("L_UpperArm");
Node* leftForeArm = GetNode("L_Forearm");
Node* leftHand = GetNode("L_Hand");
Node* rightClavicle = GetNode("R_Clavicle");
Node* rightUpperArm = GetNode("R_UpperArm");
Node* rightForeArm = GetNode("R_Forearm");
Node* rightHand = GetNode("R_Hand");
Node* leftThigh = GetNode("L_Thigh");
Node* leftCalf = GetNode("L_Calf");
Node* leftFoot = GetNode("L_Foot");
Node* leftToe = GetNode("L_Toe");
Node* rightThigh = GetNode("R_Thigh");
Node* rightCalf = GetNode("R_Calf");
Node* rightFoot = GetNode("R_Foot");
Node* rightToe = GetNode("R_Toe");
biped->AttachChild(pelvis);
pelvis->AttachChild(spine);
spine->AttachChild(spine1);
spine1->AttachChild(spine2);
spine2->AttachChild(spine3);
spine3->AttachChild(neck);
neck->AttachChild(head);
// head->AttachChild(hair);
neck->AttachChild(leftClavicle);
leftClavicle->AttachChild(leftUpperArm);
leftUpperArm->AttachChild(leftForeArm);
leftForeArm->AttachChild(leftHand);
// leftUpperArm->AttachChild(leftArm);
neck->AttachChild(rightClavicle);
rightClavicle->AttachChild(rightUpperArm);
rightUpperArm->AttachChild(rightForeArm);
rightForeArm->AttachChild(rightHand);
// rightUpperArm->AttachChild(rightArm);
// spine3->AttachChild(face);
pelvis->AttachChild(leftThigh);
leftThigh->AttachChild(leftCalf);
leftCalf->AttachChild(leftFoot);
leftFoot->AttachChild(leftToe);
// leftCalf->AttachChild(leftShoe);
// leftThigh->AttachChild(leftLeg);
// leftThigh->AttachChild(leftAngle);
pelvis->AttachChild(rightThigh);
rightThigh->AttachChild(rightCalf);
rightCalf->AttachChild(rightFoot);
rightFoot->AttachChild(rightToe);
// rightCalf->AttachChild(rightShoe);
// rightThigh->AttachChild(rightLeg);
// rightThigh->AttachChild(rightAnkle);
// pelvis->AttachChild(shirt);
// pelvis->AttachChild(pants);
// The vertex format is shared among all the triangle meshes.
mVFormat = VertexFormat::Create(2,
VertexFormat::AU_POSITION, VertexFormat::AT_FLOAT3, 0,
VertexFormat::AU_NORMAL, VertexFormat::AT_FLOAT3, 0);
// The TriMesh objects must be created after the Node tree is built,
// because the TriMesh objects have to find the "bones" that correspond
// to them.
TriMesh* hair = GetMesh("Hair",biped);
TriMesh* leftArm = GetMesh("L_Arm",biped);
TriMesh* rightArm = GetMesh("R_Arm",biped);
TriMesh* face = GetMesh("Face",biped);
TriMesh* leftShoe = GetMesh("L_Shoe",biped);
TriMesh* leftLeg = GetMesh("L_Leg",biped);
TriMesh* leftAngle = GetMesh("L_Ankle",biped);
TriMesh* rightShoe = GetMesh("R_Shoe",biped);
TriMesh* rightLeg = GetMesh("R_Leg",biped);
TriMesh* rightAnkle = GetMesh("R_Ankle",biped);
TriMesh* shirt = GetMesh("Shirt",biped);
TriMesh* pants = GetMesh("Pants",biped);
head->AttachChild(hair);
leftUpperArm->AttachChild(leftArm);
rightUpperArm->AttachChild(rightArm);
spine3->AttachChild(face);
leftCalf->AttachChild(leftShoe);
leftThigh->AttachChild(leftLeg);
leftThigh->AttachChild(leftAngle);
rightCalf->AttachChild(rightShoe);
rightThigh->AttachChild(rightLeg);
rightThigh->AttachChild(rightAnkle);
pelvis->AttachChild(shirt);
pelvis->AttachChild(pants);
mScene = new0 Node();
mScene->LocalTransform.SetRotate(HMatrix(AVector::UNIT_Z,
0.25f*Mathf::PI));
mScene->AttachChild(biped);
mScene->Update();
#if 0
// For regenerating the biped WMOF whenever engine streaming changes.
OutStream target;
target.Insert(mScene);
target.Save("SkinnedBipedPN.wmof");
#endif
}
/** @brief do a single iteration, returns the new error distance*/
double InvKinematic::Step() {
ConstructJacobian();
const int end = pivot_count_ - 1; // index of last pivot in chain
HMatrix H_0_s = HMatrix(pivots_[pivot_count_ - 1]->GetRelativeHMatrixArray());
// jacobianT_ stored as:
// x y z
// x y z
// x y z
// x y z
// normal jacobian should be
// x x x x
// y y y y
// z z z z
// calculate J * transpose(J)
// double JJT[end*3]; // k rows x 3 columns
// int r = 0;
// for (m = 0; m < 3; ++m) {
// for (n = 0; n < end; ++n) {
// JJT[m, n] = 0;
// for (trow = 0; trow < end; ++trow) {
// JJT[3*m + n] += jacobianT_[3*trow + m] * jacobianT_[3*trow + n];
// }
// }
// }
// calculate e = columnvector(e_x, e_y, e_z)
// printf("ik H_0_s: %f, %f, %f\n", H_0_s.GetX(), H_0_s.GetY(), H_0_s.GetZ());
double e[3] = {
target_[0] - H_0_s.GetX(),
target_[1] - H_0_s.GetY(),
target_[2] - H_0_s.GetZ(),
};
// printf(" error\n %f\n %f\n %f\n", e[0], e[1], e[2]);
// calc JT * e
double JTe[end]; // column vector
// printf(" JTe:\n");
for (int i = 0; i < end; ++i) {
JTe[i] = jacobianT_[3*i] * e[0] + jacobianT_[3*i + 1] * e[1] +
jacobianT_[3*i + 2] * e[2];
// printf(" %f\n", JTe[i]);
}
// calc J*JT*e
double JJTe[3];
// printf(" JJTe:\n");
for (int m = 0; m < 3; ++m) {
JJTe[m] = 0.0;
for (int n = 0; n < end; ++n) {
JJTe[m] += jacobianT_[3*n + m] * JTe[n];
}
// printf(" %f\n", JJTe[m]);
}
// alpha = (e * JJTe) / (JJTe * JJTe)
double divisor = (JJTe[0] * JJTe[0] + JJTe[1] * JJTe[1] + JJTe[2] * JJTe[2]);
double alpha;
if (divisor == 0.0) {
alpha = 0.0;
// printf(" alpha = 0.0 (divisor was 0)\n");
} else {
alpha = (e[0]*JJTe[0] + e[1] * JJTe[1] + e[2] * JJTe[2]) / divisor;
// printf(" alpha: %f\n", alpha);
}
// change angles by alpha * JTe
for (int i = 0; i < end; ++i) {
bool success = pivots_[i]->ChangeAngle(alpha * JTe[i]);
if (!success) {
flag_ = NEW_ANGLE_OUT_OF_REACH;
}
}
// recalc e
H_0_s = HMatrix(pivots_[pivot_count_ - 1]->GetRelativeHMatrixArray());
e[0] = target_[0] - H_0_s.GetX();
e[1] = target_[1] - H_0_s.GetY();
e[2] = target_[2] - H_0_s.GetZ();
return std::sqrt(std::pow(e[0], 2) + std::pow(e[1], 2) + std::pow(e[2], 2));
}
//----------------------------------------------------------------------------
Node* RoughPlaneSolidBox::CreateBox ()
{
mBox = new0 Node();
float xExtent = (float)mModule.XLocExt;
float yExtent = (float)mModule.YLocExt;
float zExtent = (float)mModule.ZLocExt;
VertexFormat* vformat = VertexFormat::Create(2,
VertexFormat::AU_POSITION, VertexFormat::AT_FLOAT3, 0,
VertexFormat::AU_COLOR, VertexFormat::AT_FLOAT3, 0);
StandardMesh sm(vformat);
VertexColor3Effect* effect = new0 VertexColor3Effect();
VertexBufferAccessor vba;
Transform transform;
TriMesh* face;
int i;
// +z face
Float3 red(1.0f, 0.0f, 0.0f);
transform.SetTranslate(APoint(0.0f, 0.0f, zExtent));
sm.SetTransform(transform);
face = sm.Rectangle(2, 2, xExtent, yExtent);
vba.ApplyTo(face);
for (i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, 0) = red;
vba.Color<Float3>(0, 1) = red;
vba.Color<Float3>(0, 2) = red;
vba.Color<Float3>(0, 3) = red;
}
face->SetEffectInstance(effect->CreateInstance());
mBox->AttachChild(face);
// -z face
Float3 darkRed(0.5f, 0.0f, 0.0f);
transform.SetTranslate(APoint(0.0f, 0.0f, -zExtent));
transform.SetRotate(HMatrix(AVector::UNIT_Y, AVector::UNIT_X,
-AVector::UNIT_Z, APoint::ORIGIN, true));
sm.SetTransform(transform);
face = sm.Rectangle(2, 2, yExtent, xExtent);
vba.ApplyTo(face);
for (i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, 0) = darkRed;
vba.Color<Float3>(0, 1) = darkRed;
vba.Color<Float3>(0, 2) = darkRed;
vba.Color<Float3>(0, 3) = darkRed;
}
face->SetEffectInstance(effect->CreateInstance());
mBox->AttachChild(face);
// +y face
Float3 green(0.0f, 1.0f, 0.0f);
transform.SetTranslate(APoint(0.0f, yExtent, 0.0f));
transform.SetRotate(HMatrix(AVector::UNIT_Z, AVector::UNIT_X,
AVector::UNIT_Y, APoint::ORIGIN, true));
sm.SetTransform(transform);
face = sm.Rectangle(2, 2, zExtent, xExtent);
vba.ApplyTo(face);
for (i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, 0) = green;
vba.Color<Float3>(0, 1) = green;
vba.Color<Float3>(0, 2) = green;
vba.Color<Float3>(0, 3) = green;
}
face->SetEffectInstance(effect->CreateInstance());
mBox->AttachChild(face);
// -y face
Float3 darkGreen(0.0f, 1.0f, 0.0f);
transform.SetTranslate(APoint(0.0f, -yExtent, 0.0f));
transform.SetRotate(HMatrix(AVector::UNIT_X, AVector::UNIT_Z,
-AVector::UNIT_Y, APoint::ORIGIN, true));
sm.SetTransform(transform);
face = sm.Rectangle(2, 2, xExtent, zExtent);
vba.ApplyTo(face);
for (i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, 0) = darkGreen;
vba.Color<Float3>(0, 1) = darkGreen;
vba.Color<Float3>(0, 2) = darkGreen;
vba.Color<Float3>(0, 3) = darkGreen;
}
face->SetEffectInstance(effect->CreateInstance());
mBox->AttachChild(face);
// +x face
Float3 blue(0.0f, 0.0f, 1.0f);
transform.SetTranslate(APoint(xExtent, 0.0f, 0.0f));
transform.SetRotate(HMatrix(AVector::UNIT_Y, AVector::UNIT_Z,
AVector::UNIT_X, APoint::ORIGIN, true));
sm.SetTransform(transform);
face = sm.Rectangle(2, 2, yExtent, zExtent);
vba.ApplyTo(face);
for (i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, 0) = blue;
vba.Color<Float3>(0, 1) = blue;
vba.Color<Float3>(0, 2) = blue;
vba.Color<Float3>(0, 3) = blue;
}
face->SetEffectInstance(effect->CreateInstance());
mBox->AttachChild(face);
// -x face
Float3 darkBlue(0.0f, 0.0f, 1.0f);
transform.SetTranslate(APoint(-xExtent, 0.0f, 0.0f));
transform.SetRotate(HMatrix(AVector::UNIT_Z, AVector::UNIT_Y,
-AVector::UNIT_X, APoint::ORIGIN, true));
sm.SetTransform(transform);
face = sm.Rectangle(2, 2, zExtent, yExtent);
vba.ApplyTo(face);
for (i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, 0) = darkBlue;
vba.Color<Float3>(0, 1) = darkBlue;
vba.Color<Float3>(0, 2) = darkBlue;
vba.Color<Float3>(0, 3) = darkBlue;
}
face->SetEffectInstance(effect->CreateInstance());
mBox->AttachChild(face);
MoveBox();
return mBox;
}
//----------------------------------------------------------------------------
void BillboardNodes::CreateScene ()
{
mScene = new0 Node();
mCullState = new0 CullState();
mRenderer->SetOverrideCullState(mCullState);
mWireState = new0 WireState();
mRenderer->SetOverrideWireState(mWireState);
// All triangle meshes have this common vertex format. Use StandardMesh
// to create these meshes.
VertexFormat* vformat = VertexFormat::Create(2,
VertexFormat::AU_POSITION, VertexFormat::AT_FLOAT3, 0,
VertexFormat::AU_TEXCOORD, VertexFormat::AT_FLOAT2, 0);
StandardMesh stdMesh(vformat);
// Create the ground. It covers a square with vertices (1,1,0), (1,-1,0),
// (-1,1,0), and (-1,-1,0). Multiply the texture coordinates by a factor
// to enhance the wrap-around.
mGround = stdMesh.Rectangle(2, 2, 16.0f, 16.0f);
VertexBufferAccessor vba(mGround);
int i;
for (i = 0; i < vba.GetNumVertices(); ++i)
{
Float2& tcoord = vba.TCoord<Float2>(0, i);
tcoord[0] *= 128.0f;
tcoord[1] *= 128.0f;
}
// Create a texture effect for the ground.
std::string path = Environment::GetPathR("Horizontal.wmtf");
Texture2D* texture = Texture2D::LoadWMTF(path);
VisualEffectInstance* instance = Texture2DEffect::CreateUniqueInstance(
texture, Shader::SF_LINEAR_LINEAR, Shader::SC_REPEAT,
Shader::SC_REPEAT);
mGround->SetEffectInstance(instance);
mScene->AttachChild(mGround);
// Create a rectangle mesh. The mesh is in the xy-plane. Do not apply
// local transformations to the mesh. Use the billboard node transforms
// to control the mesh location and orientation.
mRectangle = stdMesh.Rectangle(2, 2, 0.125f, 0.25f);
// Create a texture effect for the rectangle and for the torus.
Texture2DEffect* geomEffect = new0 Texture2DEffect(Shader::SF_LINEAR);
path = Environment::GetPathR("RedSky.wmtf");
texture = Texture2D::LoadWMTF(path);
mRectangle->SetEffectInstance(geomEffect->CreateInstance(texture));
// Create a billboard node that causes a rectangle to always be facing
// the camera. This is the type of billboard for an avatar.
mBillboard0 = new0 BillboardNode(mCamera);
mBillboard0->AttachChild(mRectangle);
mScene->AttachChild(mBillboard0);
// The billboard rotation is about its model-space up-vector (0,1,0). In
// this application, world-space up is (0,0,1). Locally rotate the
// billboard so it's up-vector matches the world's.
mBillboard0->LocalTransform.SetTranslate(APoint(-0.25f, 0.0f, 0.25f));
mBillboard0->LocalTransform.SetRotate(HMatrix(AVector::UNIT_X,
Mathf::HALF_PI));
// Create a torus mesh. Do not apply local transformations to the mesh.
// Use the billboard node transforms to control the mesh location and
// orientation.
mTorus = StandardMesh(vformat, false).Torus(16, 16, 1.0f, 0.25f);
mTorus->LocalTransform.SetUniformScale(0.1f);
// Create a texture effect for the torus. It uses the RedSky image that
// the rectangle uses.
mTorus->SetEffectInstance(geomEffect->CreateInstance(texture));
// Create a billboard node that causes an object to always be oriented
// the same way relative to the camera.
mBillboard1 = new0 BillboardNode(mCamera);
mBillboard1->AttachChild(mTorus);
mScene->AttachChild(mBillboard1);
// The billboard rotation is about its model-space up-vector (0,1,0). In
// this application, world-space up is (0,0,1). Locally rotate the
// billboard so it's up-vector matches the world's.
mBillboard1->LocalTransform.SetTranslate(APoint(0.25f, 0.0f, 0.25f));
mBillboard1->LocalTransform.SetRotate(HMatrix(AVector::UNIT_X,
Mathf::HALF_PI));
#ifdef DEMONSTRATE_VIEWPORT_BOUNDING_RECTANGLE
// The screen camera is designed to map (x,y,z) in [0,1]^3 to (x',y,'z')
// in [-1,1]^2 x [0,1].
mSSCamera = new0 Camera(false);
mSSCamera->SetFrustum(0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f);
mSSCamera->SetFrame(APoint::ORIGIN, AVector::UNIT_Z, AVector::UNIT_Y,
AVector::UNIT_X);
// Create a semitransparent screen rectangle.
VertexFormat* ssVFormat = VertexFormat::Create(2,
VertexFormat::AU_POSITION, VertexFormat::AT_FLOAT3, 0,
VertexFormat::AU_COLOR, VertexFormat::AT_FLOAT4, 0);
int ssVStride = ssVFormat->GetStride();
VertexBuffer* ssVBuffer = new0 VertexBuffer(4, ssVStride);
VertexBufferAccessor ssVba(ssVFormat, ssVBuffer);
Float4 ssColor(0.0f, 0.0f, 1.0f, 0.25f);
ssVba.Position<Float3>(0) = Float3(0.0f, 0.0f, 0.0f);
ssVba.Position<Float3>(1) = Float3(1.0f, 0.0f, 0.0f);
ssVba.Position<Float3>(2) = Float3(1.0f, 1.0f, 0.0f);
ssVba.Position<Float3>(3) = Float3(0.0f, 1.0f, 0.0f);
ssVba.Color<Float4>(0, 0) = ssColor;
ssVba.Color<Float4>(0, 1) = ssColor;
ssVba.Color<Float4>(0, 2) = ssColor;
ssVba.Color<Float4>(0, 3) = ssColor;
IndexBuffer* ssIBuffer = new0 IndexBuffer(6, sizeof(int));
int* indices = (int*)ssIBuffer->GetData();
indices[0] = 0; indices[1] = 1; indices[2] = 2;
indices[3] = 0; indices[4] = 2; indices[5] = 3;
mSSRectangle = new0 TriMesh(ssVFormat, ssVBuffer, ssIBuffer);
mSSRectangle->Update();
// Create a vertex color effect for the screen rectangle.
VertexColor4Effect* ssEffect = new0 VertexColor4Effect();
mSSRectangle->SetEffectInstance(ssEffect->CreateInstance());
// Alpha blending must be enabled to obtain the semitransparency.
ssEffect->GetAlphaState(0, 0)->BlendEnabled = true;
#endif
}
//----------------------------------------------------------------------------
bool NonlocalBlowup::OnInitialize ()
{
if (!WindowApplication3::OnInitialize())
{
return false;
}
#ifdef RUN_CONSOLE
RunConsole();
return false;
#endif
mScene = new0 Node();
mScene->LocalTransform.SetRotate(HMatrix(
0.80475128f, 0.59107417f, -0.054833174f, 0.0f,
-0.17529237f, 0.32487807f, 0.92936903f, 0.0f,
0.56714010f, -0.73829913f, 0.36505684f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f));
mWireState = new0 WireState();
mRenderer->SetOverrideWireState(mWireState);
VertexFormat* vformat = VertexFormat::Create(2,
VertexFormat::AU_POSITION, VertexFormat::AT_FLOAT3, 0,
VertexFormat::AU_TEXCOORD, VertexFormat::AT_FLOAT2, 0);
TriMesh* mesh = StandardMesh(vformat).Rectangle(256, 256, 16.0f, 16.0f);
mScene->AttachChild(mesh);
std::string gridName = Environment::GetPathR("Grid.wmtf");
Texture2D* gridTexture = Texture2D::LoadWMTF(gridName);
gridTexture->GenerateMipmaps();
Texture1D* colorTexture = new0 Texture1D(Texture::TF_A8R8G8B8, 8, 1);
unsigned char* color = (unsigned char*)colorTexture->GetData(0);
color[ 0] = 128; color[ 1] = 128; color[ 2] = 128; color[ 3] = 255;
color[ 4] = 255; color[ 5] = 0; color[ 6] = 128; color[ 7] = 255;
color[ 8] = 255; color[ 9] = 0; color[10] = 0; color[11] = 255;
color[12] = 0; color[13] = 255; color[14] = 0; color[15] = 255;
color[16] = 0; color[17] = 255; color[18] = 255; color[19] = 255;
color[20] = 0; color[21] = 128; color[22] = 255; color[23] = 255;
color[24] = 0; color[25] = 0; color[26] = 255; color[27] = 255;
color[28] = 255; color[29] = 255; color[30] = 255; color[31] = 255;
float dt = 0.01f, dx = 1.0f, dy = 1.0f;
// Uncomment only one of these at a time.
NonconvexDomain0p50(dt, dx, dy);
//SquareSymmetric0p01(dt, dx, dy);
//SquareSymmetric0p50(dt, dx, dy);
//SquareSymmetric0p99(dt, dx, dy);
//SquareGaussX0p50(dt, dx, dy);
//SquareGaussXY0p50(dt, dx, dy);
//SquareGaussFour0p50(dt, dx, dy);
DisplacementEffect* effect = new0 DisplacementEffect();
mesh->SetEffectInstance(effect->CreateInstance(mHeightTexture,
gridTexture, colorTexture, mDomainTexture));
// Set up the camera so that it looks at the graph from slightly above
// the xy-plane and at a skewed angle/direction of view.
mCamera->SetFrustum(60.0f, GetAspectRatio(), 1.0f, 10000.0f);
APoint camPosition(0.0f, 3.46f, 42.97f);
AVector camDVector(0.0f, 0.0f, -1.0f);
AVector camUVector(0.0f, 1.0f, 0.0f);
AVector camRVector = camDVector.Cross(camUVector);
mCamera->SetFrame(camPosition, camDVector, camUVector, camRVector);
// Initial update of objects.
mScene->Update();
// Initial culling of scene.
mCuller.SetCamera(mCamera);
mCuller.ComputeVisibleSet(mScene);
InitializeCameraMotion(0.01f, 0.01f);
InitializeObjectMotion(mScene);
return true;
}
