dCustomInverseDynamicsEffector* AddLeg(DemoEntityManager* const scene, void* const rootNode, const dMatrix& matrix, dFloat partMass, dFloat limbLenght)
{
NewtonBody* const parent = NewtonInverseDynamicsGetBody(m_kinematicSolver, rootNode);
dFloat inertiaScale = 4.0f;
// make limb base
dMatrix baseMatrix(dRollMatrix(dPi * 0.5f));
dMatrix cylinderMatrix(baseMatrix * matrix);
NewtonBody* const base = CreateCylinder(scene, cylinderMatrix, partMass, inertiaScale, 0.2f, 0.1f);
dCustomInverseDynamics* const baseHinge = new dCustomInverseDynamics(cylinderMatrix, base, parent);
baseHinge->SetJointTorque(1000.0f);
baseHinge->SetTwistAngle(-0.5f * dPi, 0.5f * dPi);
void* const baseHingeNode = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, rootNode, baseHinge->GetJoint());
//make limb forward arm
dMatrix forwardArmMatrix(dPitchMatrix(-30.0f * dDegreeToRad));
dVector forwardArmSize(limbLenght * 0.25f, limbLenght * 0.25f, limbLenght, 0.0f);
forwardArmMatrix.m_posit += forwardArmMatrix.m_right.Scale(forwardArmSize.m_z * 0.5f);
NewtonBody* const forwardArm = CreateBox(scene, forwardArmMatrix * matrix, forwardArmSize, partMass, inertiaScale);
dMatrix forwardArmPivot(forwardArmMatrix);
forwardArmPivot.m_posit -= forwardArmMatrix.m_right.Scale(forwardArmSize.m_z * 0.5f);
dCustomInverseDynamics* const forwardArmHinge = new dCustomInverseDynamics(forwardArmPivot * matrix, forwardArm, base);
forwardArmHinge->SetJointTorque(1000.0f);
forwardArmHinge->SetTwistAngle(-0.5f * dPi, 0.5f * dPi);
void* const forwardArmHingeNode = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, baseHingeNode, forwardArmHinge->GetJoint());
//make limb forward arm
dMatrix armMatrix(dPitchMatrix(-90.0f * dDegreeToRad));
dFloat armSize = limbLenght * 1.25f;
armMatrix.m_posit += forwardArmMatrix.m_right.Scale(limbLenght);
armMatrix.m_posit.m_y -= armSize * 0.5f;
NewtonBody* const arm = CreateCapsule(scene, armMatrix * matrix, partMass, inertiaScale, armSize * 0.2f, armSize);
dMatrix armPivot(armMatrix);
armPivot.m_posit.m_y += armSize * 0.5f;
dCustomInverseDynamics* const armHinge = new dCustomInverseDynamics(armPivot * matrix, arm, forwardArm);
armHinge->SetJointTorque(1000.0f);
armHinge->SetTwistAngle(-0.5f * dPi, 0.5f * dPi);
void* const armHingeNode = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, forwardArmHingeNode, armHinge->GetJoint());
dMatrix effectorMatrix(dGetIdentityMatrix());
effectorMatrix.m_posit = armPivot.m_posit;
effectorMatrix.m_posit.m_y -= armSize;
dHexapodEffector* const effector = new dHexapodEffector(m_kinematicSolver, armHingeNode, parent, effectorMatrix * matrix);
effector->SetAsThreedof();
effector->SetMaxLinearFriction(partMass * DEMO_GRAVITY * 10.0f);
effector->SetMaxAngularFriction(partMass * DEMO_GRAVITY * 10.0f);
return effector;
}
int CreateCone (dVector* const points, dVector* const normals, int segments, dFloat radius, dFloat height, int maxPoints)
{
dMatrix rotation (dPitchMatrix((-360.0f / segments) * 3.141592f/180.0f));
dVector p0 (0.0, 0.0f, 0.0f, 0.0f);
dVector p1 (0.0, radius, 0.0f, 0.0f);
dVector q1 (height, 0.0f, 0.0f, 0.0f);
dVector n (radius, height, 0.0f, 0.0f);
n = n.Scale (1.0f / dSqrt (n.DotProduct3(n)));
int count = 0;
for (int i = 0; (i < segments) && (count < maxPoints); i ++) {
dVector p2 (rotation.RotateVector(p1));
normals[count] = dVector (-1.0f, 0.0f, 0.0f, 0.0f);
points[count * 3 + 0] = p0;
points[count * 3 + 1] = p1;
points[count * 3 + 2] = p2;
count ++;
normals[count] = dVector (n.m_x, n.m_y, n.m_z, 0.0f);
points[count * 3 + 0] = p1;
points[count * 3 + 1] = q1;
points[count * 3 + 2] = p2;
count ++;
n = rotation.RotateVector(n);
p1 = p2;
}
return count;
}
dMatrix::dMatrix (dFloat pitch, dFloat yaw, dFloat roll, const dVector & location)
{
dMatrix & me = *this;
me = dPitchMatrix (pitch) * dYawMatrix (yaw) * dRollMatrix (roll);
me.m_posit = location;
me.m_posit.m_w = 1.0f;
}
XXXX ()
{
dMatrix s;
dFloat m = 2.0f;
for (int i = 0; i < 3; i ++)
{
for (int j = 0; j < 3; j ++)
{
s[i][j] = m;
m += (i + 1) + j;
}
}
s.m_posit = dVector (1, 2, 3, 1);
dMatrix matrix;
dVector scale;
dMatrix stretch;
s.PolarDecomposition (matrix, scale, stretch);
dMatrix s1 (matrix, scale, stretch);
dMatrix xxx (dPitchMatrix (30.0f * 3.14159f / 180.0f) * dRollMatrix (30.0f * 3.14159f / 180.0f));
dMatrix xxxx (GetIdentityMatrix());
xxx[0] = xxx[0].Scale (-1.0f);
dFloat mmm = (xxx[0] * xxx[1]) % xxx[2];
xxxx[0][0] = 3.0f;
xxxx[1][1] = 3.0f;
xxxx[2][2] = 4.0f;
dMatrix xxx2 (xxx * xxxx);
mmm = (xxx2[0] * xxx2[1]) % xxx2[2];
xxx2.PolarDecomposition (matrix, scale, stretch);
s1 = dMatrix (matrix, scale, stretch);
s1 = dMatrix (matrix, scale, stretch);
}
void pyScene::Save (const char* name)
{
dMatrix globalRotation (dPitchMatrix(3.14159265f * 0.5f) * dRollMatrix(3.14159265f * 0.5f));
globalRotation = globalRotation.Inverse();
m_scene->BakeTransform (globalRotation);
m_scene->Serialize (name);
}
// this calculate the desired position and orientation in the path
dMatrix BuildMatrix (dFloat timestep)
{
dMatrix matrix (dPitchMatrix(-m_angle) * dRollMatrix(-m_angle));
matrix.m_posit = m_origin;
matrix.m_posit.m_z += radius * dSin (m_angle);
matrix.m_posit.m_y += radius * dCos (m_angle);
return matrix;
}
dMatrix CustomDGRayCastCar::CalculateTireMatrix (int tireIndex) const
{
const Tire& tire = m_tires[tireIndex];
// calculate the rotation angle matrix
dMatrix angleMatrix ( dPitchMatrix( tire.m_spinAngle ) );
// get the tire body matrix
dMatrix bodyMatrix;
NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
return angleMatrix * CalculateSuspensionMatrix (tireIndex, tire.m_posit) * m_localFrame * bodyMatrix;
}
virtual void Evaluate(dEffectorPose& output, dFloat timestep)
{
m_poseGenerator->Evaluate(output, timestep);
dQuaternion rotation (dPitchMatrix(m_euler.m_x) * dYawMatrix(m_euler.m_y) * dRollMatrix(m_euler.m_z));
for (dEffectorPose::dListNode* node = output.GetFirst(); node; node = node->GetNext()) {
dEffectorTransform& transform = node->GetInfo();
transform.m_rotation = transform.m_rotation * rotation;
transform.m_posit = m_position + rotation.RotateVector(transform.m_posit);
}
}
Import::Import(const char* pathName, Interface* ip, ImpInterface* impip)
{
// set the path for textures
char* ptr = NULL;
sprintf (m_path, "%s", pathName);
for (int i = 0; m_path[i]; i ++) {
if ((m_path[i] == '\\') || (m_path[i] == '/') ) {
ptr = &m_path[i];
}
}
*ptr = 0;
m_ip = ip;
m_impip = impip;
m_succes = TRUE;
MaterialCache materialCache (NewDefaultMultiMtl());
SetSceneParameters();
dFloat scale;
scale = float (GetMasterScale(UNITS_METERS));
dMatrix scaleMatrix (GetIdentityMatrix());
scaleMatrix[0][0] = 1.0f / scale;
scaleMatrix[1][1] = 1.0f / scale;
scaleMatrix[2][2] = 1.0f / scale;
dMatrix globalRotation (scaleMatrix * dPitchMatrix(3.14159265f * 0.5f) * dRollMatrix(3.14159265f * 0.5f));
NewtonWorld* newton = NewtonCreate();
dScene scene (newton);
scene.Deserialize (pathName);
// dScene::Iterator iter (scene);
// for (iter.Begin(); iter; iter ++) {
// dScene::dTreeNode* node = iter.GetNode();
// dNodeInfo* info = scene.GetInfoFromNode(node);
// if (info->GetTypeId() == dMeshNodeInfo::GetRttiType()) {
// dMeshNodeInfo* mesh = (dMeshNodeInfo*) scene.GetInfoFromNode(node);
// mesh->ConvertToTriangles();
// }
// }
scene.BakeTransform (globalRotation);
GeometryCache geometryCache;
MaxNodeChache maxNodeCache;
LoadMaterials (scene, materialCache);
LoadGeometries (scene, geometryCache, materialCache);
LoadNodes (scene, geometryCache, materialCache.m_multiMat, maxNodeCache);
ApplyModifiers (scene, maxNodeCache);
scene.CleanUp();
NewtonDestroy(newton);
}
dAnimationJoint* CreateChildNode(NewtonBody* const boneBody, dAnimationJoint* const parent, const dJointDefinition& definition) const
{
dMatrix matrix;
NewtonBodyGetMatrix(boneBody, &matrix[0][0]);
dJointDefinition::dFrameMatrix frameAngle(definition.m_frameBasics);
dMatrix pinAndPivotInGlobalSpace(dPitchMatrix(frameAngle.m_pitch * dDegreeToRad) * dYawMatrix(frameAngle.m_yaw * dDegreeToRad) * dRollMatrix(frameAngle.m_roll * dDegreeToRad));
pinAndPivotInGlobalSpace = pinAndPivotInGlobalSpace * matrix;
//dMatrix bindMatrix(entity->GetParent()->CalculateGlobalMatrix((DemoEntity*)NewtonBodyGetUserData(parentBody)).Inverse());
dMatrix bindMatrix(dGetIdentityMatrix());
dAnimationJoint* const joint = new dAnimationRagdollJoint(definition.m_type, pinAndPivotInGlobalSpace, boneBody, bindMatrix, parent);
return joint;
}
void dSceneRender::DrawCylinder(int segments, dFloat radius, dFloat heigh)
{
dVector q0 ( heigh / 2.0f, radius, 0.0f, 0.0f);
dVector q1 (-heigh / 2.0f, radius, 0.0f, 0.0f);
dMatrix rotation (dPitchMatrix(2.0f * 3.1614f/segments));
dVector cap0[1024];
dVector cap1[1024];
dAssert (segments < sizeof (cap0)/sizeof (cap0[0]));
cap0[segments] = q0;
cap1[segments] = q1;
for (int i = 0; i < segments; i ++) {
cap0[i] = q0;
cap1[i] = q1;
q0 = rotation.RotateVector(q0);
q1 = rotation.RotateVector(q1);
}
dVector normal0 ( 1.0f, 0.0f, 0.0f, 0.0f);
dVector normal1 (-1.0f, 0.0f, 0.0f, 0.0f);
BeginTriangle();
for (int i = 2; i < segments; i ++) {
SubmitNormal(normal0);
DrawTriangle(cap0[0], cap0[i-1], cap0[i]);
SubmitNormal(normal1);
DrawTriangle(cap1[0], cap1[i], cap1[i - 1]);
}
for (int i = 0; i < segments; i ++) {
dVector p0 (cap0[i]);
dVector p1 (cap0[i + 1]);
dVector p2 (cap1[i + 1]);
dVector p3 (cap1[i]);
dVector normal ((p1 - p0) * (p2 - p0));
normal = normal.Scale (1.0f / dSqrt(normal % normal));
SubmitNormal(normal);
DrawTriangle(p0, p2, p1);
SubmitNormal(normal);
DrawTriangle(p0, p3, p2);
}
End();
}
void pyScene::Load (const char* name)
{
m_scene->CleanUp();
m_scene->Deserialize(name);
dScene::Iterator iter (*m_scene);
for (iter.Begin(); iter; iter ++) {
dScene::dTreeNode* node = iter.GetNode();
dNodeInfo* info = m_scene->GetInfoFromNode(node);
if (info->GetTypeId() == dMeshNodeInfo::GetRttiType()) {
dMeshNodeInfo* mesh = (dMeshNodeInfo*) m_scene->GetInfoFromNode(node);
mesh->ConvertToTriangles();
}
}
dMatrix globalRotation (dPitchMatrix(3.14159265f * 0.5f) * dRollMatrix(3.14159265f * 0.5f));
m_scene->BakeTransform (globalRotation);
}
int CreateCylinder (dVector* const points, dVector* const normals, int segments, dFloat radius, dFloat height, int maxPoints)
{
dMatrix rotation (dPitchMatrix((-360.0f / segments) * 3.141592f/180.0f));
dVector p0 (0.0, 0.0f, 0.0f, 0.0f);
dVector p1 (0.0, radius, 0.0f, 0.0f);
dVector q0 (height, 0.0f, 0.0f, 0.0f);
dVector q1 (height, radius, 0.0f, 0.0f);
dVector n (0, 1.0f, 0.0f, 0.0f);
int count = 0;
for (int i = 0; (i < segments) && (count < maxPoints); i ++) {
dVector p2 (rotation.RotateVector(p1));
normals[count] = dVector (-1.0f, 0.0f, 0.0f, 0.0f);
points[count * 3 + 0] = p0;
points[count * 3 + 1] = p1;
points[count * 3 + 2] = p2;
count ++;
dVector q2 (rotation.RotateVector(q1));
normals[count] = dVector (1.0f, 0.0f, 0.0f, 0.0f);
points[count * 3 + 0] = q0;
points[count * 3 + 1] = q2;
points[count * 3 + 2] = q1;
count ++;
normals[count] = n;
points[count * 3 + 0] = p1;
points[count * 3 + 1] = q1;
points[count * 3 + 2] = p2;
count ++;
normals[count] = n;
points[count * 3 + 0] = p2;
points[count * 3 + 1] = q1;
points[count * 3 + 2] = q2;
count ++;
n = rotation.RotateVector(n);
p1 = p2;
q1 = q2;
}
return count;
}
void dCustomCorkScrew::Debug(dDebugDisplay* const debugDisplay) const
{
dCustomSlider::Debug(debugDisplay);
if (m_options.m_option2) {
dMatrix matrix0;
dMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
const int subdiv = 12;
dVector arch[subdiv + 1];
const dFloat radius = debugDisplay->m_debugScale;
if ((m_maxAngle > 1.0e-3f) || (m_minAngle < -1.0e-3f)) {
// show pitch angle limits
dVector point(dFloat(0.0f), dFloat(radius), dFloat(0.0f), dFloat(0.0f));
dFloat minAngle = m_minAngle;
dFloat maxAngle = m_maxAngle;
if ((maxAngle - minAngle) >= dPi * 2.0f) {
minAngle = 0.0f;
maxAngle = dPi * 2.0f;
}
dFloat angleStep = (maxAngle - minAngle) / subdiv;
dFloat angle0 = minAngle;
matrix1.m_posit = matrix0.m_posit;
debugDisplay->SetColor(dVector(0.5f, 0.0f, 0.0f, 0.0f));
for (int i = 0; i <= subdiv; i++) {
arch[i] = matrix1.TransformVector(dPitchMatrix(angle0).RotateVector(point));
debugDisplay->DrawLine(matrix1.m_posit, arch[i]);
angle0 += angleStep;
}
for (int i = 0; i < subdiv; i++) {
debugDisplay->DrawLine(arch[i], arch[i + 1]);
}
}
}
}
void PostUpdate(dFloat timestep, int threadIndex)
{
dMatrix matrixA;
NewtonBodyGetMatrix(m_body, &matrixA[0][0]);
dFloat speed = m_step * timestep * 60.0f;
m_pith = dMod (m_pith + speed, 3.1416f * 2.0f);
m_yaw = dMod (m_yaw + speed, 3.1416f * 2.0f);
m_roll = dMod (m_roll + speed, 3.1416f * 2.0f);
dMatrix matrixB(dPitchMatrix(m_pith) * dYawMatrix(m_yaw) * dRollMatrix(m_roll));
matrixB.m_posit = matrixA.m_posit;
matrixB.m_posit.m_y = 5.0f;
NewtonWorld* const world = NewtonBodyGetWorld(m_body);
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData (world);
m_castingVisualEntity->ResetMatrix(*scene, matrixB);
NewtonCollision* const collisionA = NewtonBodyGetCollision(m_body);
NewtonCollisionClosestPoint(world, collisionA, &matrixA[0][0], m_castingVisualEntity->m_castingShape, &matrixB[0][0], &m_castingVisualEntity->m_contact0[0], &m_castingVisualEntity->m_contact1[0], &m_castingVisualEntity->m_normal[0], 0);
}
void PostUpdate(dFloat timestep, int threadIndex)
{
dMatrix matrixA;
NewtonBodyGetMatrix(m_body, &matrixA[0][0]);
dFloat speed = m_step * timestep * 60.0f;
m_pith = dMod (m_pith + speed, dPi * 2.0f);
m_yaw = dMod (m_yaw + speed, dPi * 2.0f);
m_roll = dMod (m_roll + speed, dPi * 2.0f);
dMatrix matrixB(dPitchMatrix(m_pith) * dYawMatrix(m_yaw) * dRollMatrix(m_roll));
matrixB.m_posit = matrixA.m_posit;
matrixB.m_posit.m_y = 5.0f;
//matrixB.m_posit.m_y = 1.5f;
NewtonWorld* const world = NewtonBodyGetWorld(m_body);
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData (world);
m_castingVisualEntity->ResetMatrix(*scene, matrixB);
NewtonCollision* const collisionA = NewtonBodyGetCollision(m_body);
int res = NewtonCollisionClosestPoint(world, collisionA, &matrixA[0][0], m_castingVisualEntity->m_castingShape, &matrixB[0][0], &m_castingVisualEntity->m_contact0[0], &m_castingVisualEntity->m_contact1[0], &m_castingVisualEntity->m_normal[0], 0);
//just test the center of collisionB against collisionA to see if the point is inside or not:
//int res = NewtonCollisionPointDistance(world, &matrixA.m_posit[0], collisionA, &matrixA[0][0], &m_castingVisualEntity->m_contact0[0], &m_castingVisualEntity->m_normal[0], 0);
if (res == 0) {
printf("Point Inside Body!\n");
//dTrace(("Point Inside Body!\n"));
}
else
{
//printf("Point point outside Body!\n");
}
}
//dAnimTakeData* LoadAnimation(dAnimIKController* const controller, const char* const animName)
dAnimationKeyframesSequence* GetAnimationSequence(const char* const animName)
{
dTree<dAnimationKeyframesSequence*, dString>::dTreeNode* cachedAnimNode = m_animCache.Find(animName);
if (!cachedAnimNode) {
dScene scene(GetWorld());
char pathName[2048];
dGetWorkingFileName(animName, pathName);
scene.Deserialize(pathName);
dScene::dTreeNode* const animTakeNode = scene.FindChildByType(scene.GetRootNode(), dAnimationTake::GetRttiType());
if (animTakeNode) {
//dTree<dAnimimationKeyFramesTrack*, dString> map;
//const dAnimPose& basePose = controller->GetBasePose();
//dAnimTakeData* const animdata = new dAnimTakeData(basePose.GetCount());
dAnimationKeyframesSequence* const animdata = new dAnimationKeyframesSequence();
dAnimationTake* const animTake = (dAnimationTake*)scene.GetInfoFromNode(animTakeNode);
animdata->SetPeriod(animTake->GetPeriod());
cachedAnimNode = m_animCache.Insert(animdata, animName);
//dList<dAnimimationKeyFramesTrack>& tracks = animdata->GetTracks();
//dList<dAnimimationKeyFramesTrack>::dListNode* ptr = tracks.GetFirst();
//dList<dAnimimationKeyFramesTrack>::dListNode* ptr = tracks.Append();
//for (dAnimPose::dListNode* ptrNode = basePose.GetFirst(); ptrNode; ptrNode = ptrNode->GetNext()) {
// DemoEntity* const entity = (DemoEntity*)ptrNode->GetInfo().m_userData;
// map.Insert(&ptr->GetInfo(), entity->GetName());
// ptr = ptr->GetNext();
//}
dList<dAnimimationKeyFramesTrack>& tracks = animdata->GetTracks();
for (void* link = scene.GetFirstChildLink(animTakeNode); link; link = scene.GetNextChildLink(animTakeNode, link)) {
dScene::dTreeNode* const node = scene.GetNodeFromLink(link);
dAnimationTrack* const srcTrack = (dAnimationTrack*)scene.GetInfoFromNode(node);
if (srcTrack->IsType(dAnimationTrack::GetRttiType())) {
//dTree<dAnimimationKeyFramesTrack*, dString>::dTreeNode* const ptrNode = map.Find(srcTrack->GetName());
//dAssert(ptrNode);
//dAnimTakeData::dAnimTakeTrack* const dstTrack = ptrNode->GetInfo();
dAnimimationKeyFramesTrack* const dstTrack = &tracks.Append()->GetInfo();
dstTrack->SetName(srcTrack->GetName());
const dList<dAnimationTrack::dCurveValue>& rotations = srcTrack->GetRotations();
dstTrack->m_rotation.Resize(rotations.GetCount());
int index = 0;
for (dList<dAnimationTrack::dCurveValue>::dListNode* node = rotations.GetFirst(); node; node = node->GetNext()) {
dAnimationTrack::dCurveValue keyFrame(node->GetInfo());
dMatrix matrix(dPitchMatrix(keyFrame.m_x) * dYawMatrix(keyFrame.m_y) * dRollMatrix(keyFrame.m_z));
dQuaternion rot(matrix);
dstTrack->m_rotation[index].m_rotation = rot;
dstTrack->m_rotation[index].m_time = keyFrame.m_time;
index++;
}
for (int i = 0; i < rotations.GetCount() - 1; i++) {
dFloat dot = dstTrack->m_rotation[i].m_rotation.DotProduct(dstTrack->m_rotation[i + 1].m_rotation);
if (dot < 0.0f) {
dstTrack->m_rotation[i + 1].m_rotation.m_x *= -1.0f;
dstTrack->m_rotation[i + 1].m_rotation.m_y *= -1.0f;
dstTrack->m_rotation[i + 1].m_rotation.m_z *= -1.0f;
dstTrack->m_rotation[i + 1].m_rotation.m_w *= -1.0f;
}
}
const dList<dAnimationTrack::dCurveValue>& positions = srcTrack->GetPositions();
dstTrack->m_position.Resize(positions.GetCount());
index = 0;
for (dList<dAnimationTrack::dCurveValue>::dListNode* node = positions.GetFirst(); node; node = node->GetNext()) {
dAnimationTrack::dCurveValue keyFrame(node->GetInfo());
dstTrack->m_position[index].m_posit = dVector(keyFrame.m_x, keyFrame.m_y, keyFrame.m_z, dFloat(1.0f));
dstTrack->m_position[index].m_time = keyFrame.m_time;
index++;
}
}
}
}
}
dAssert(cachedAnimNode);
return cachedAnimNode->GetInfo();
}
//dVector dMatrix::GetEulerAngles (dEulerAngleOrder order) const
void dMatrix::GetEulerAngles (dVector & euler0, dVector & euler1, dEulerAngleOrder order) const
{
int a0 = (order >> 8) & 3;
int a1 = (order >> 4) & 3;
int a2 = (order >> 0) & 3;
const dMatrix & matrix = *this;
// Assuming the angles are in radians.
if (matrix[a0][a2] > 0.99995f)
{
dFloat picth0 = 0.0f;
dFloat yaw0 = -3.141592f * 0.5f;
dFloat roll0 = - dAtan2 (matrix[a2][a1], matrix[a1][a1]);
euler0[a0] = picth0;
euler0[a1] = yaw0;
euler0[a2] = roll0;
euler1[a0] = picth0;
euler1[a1] = yaw0;
euler1[a2] = roll0;
}
else
if (matrix[a0][a2] < -0.99995f)
{
dFloat picth0 = 0.0f;
dFloat yaw0 = 3.141592f * 0.5f;
dFloat roll0 = dAtan2 (matrix[a2][a1], matrix[a1][a1]);
euler0[a0] = picth0;
euler0[a1] = yaw0;
euler0[a2] = roll0;
euler1[a0] = picth0;
euler1[a1] = yaw0;
euler1[a2] = roll0;
}
else
{
//euler[a0] = -dAtan2(-matrix[a1][a2], matrix[a2][a2]);
//euler[a1] = -dAsin ( matrix[a0][a2]);
//euler[a2] = -dAtan2(-matrix[a0][a1], matrix[a0][a0]);
dFloat yaw0 = -dAsin ( matrix[a0][a2]);
dFloat yaw1 = 3.141592f - yaw0;
dFloat sign0 = dSign (dCos (yaw0));
dFloat sign1 = dSign (dCos (yaw1));
dFloat picth0 = dAtan2 (matrix[a1][a2] * sign0, matrix[a2][a2] * sign0);
dFloat picth1 = dAtan2 (matrix[a1][a2] * sign1, matrix[a2][a2] * sign1);
dFloat roll0 = dAtan2 (matrix[a0][a1] * sign0, matrix[a0][a0] * sign0);
dFloat roll1 = dAtan2 (matrix[a0][a1] * sign1, matrix[a0][a0] * sign1);
if (yaw1 > 3.141592f)
yaw1 -= 2.0f * 3.141592f;
euler0[a0] = picth0;
euler0[a1] = yaw0;
euler0[a2] = roll0;
euler1[a0] = picth1;
euler1[a1] = yaw1;
euler1[a2] = roll1;
}
euler0[3] = dFloat (0.0f);
euler1[3] = dFloat (0.0f);
#ifdef _DEBUG
if (order == m_pitchYawRoll)
{
dMatrix m0 (dPitchMatrix (euler0[0]) * dYawMatrix (euler0[1]) * dRollMatrix (euler0[2]));
dMatrix m1 (dPitchMatrix (euler1[0]) * dYawMatrix (euler1[1]) * dRollMatrix (euler1[2]));
for (int i = 0; i < 3; i ++)
{
for (int j = 0; j < 3; j ++)
{
dFloat error = dAbs (m0[i][j] - matrix[i][j]);
dAssert (error < 5.0e-2f);
error = dAbs (m1[i][j] - matrix[i][j]);
dAssert (error < 5.0e-2f);
}
}
}
#endif
}
StupidComplexOfConvexShapes (DemoEntityManager* const scene, int count)
:DemoEntity (dGetIdentityMatrix(), NULL)
,m_rayP0(0.0f, 0.0f, 0.0f, 0.0f)
,m_rayP1(0.0f, 0.0f, 0.0f, 0.0f)
{
scene->Append(this);
count = 40;
//count = 1;
const dFloat size = 0.5f;
DemoMesh* gemetries[32];
NewtonCollision* collisionArray[32];
NewtonWorld* const world = scene->GetNewton();
int materialID = NewtonMaterialGetDefaultGroupID(world);
// create a pool of predefined convex mesh
// PrimitiveType selection[] = {_SPHERE_PRIMITIVE, _BOX_PRIMITIVE, _CAPSULE_PRIMITIVE, _CYLINDER_PRIMITIVE, _CONE_PRIMITIVE, _TAPERED_CAPSULE_PRIMITIVE, _TAPERED_CYLINDER_PRIMITIVE, _CHAMFER_CYLINDER_PRIMITIVE, _RANDOM_CONVEX_HULL_PRIMITIVE, _REGULAR_CONVEX_HULL_PRIMITIVE};
PrimitiveType selection[] = {_SPHERE_PRIMITIVE};
for (int i = 0; i < int (sizeof (collisionArray) / sizeof (collisionArray[0])); i ++) {
int index = dRand() % (sizeof (selection) / sizeof (selection[0]));
dVector shapeSize (size + RandomVariable (size / 2.0f), size + RandomVariable (size / 2.0f), size + RandomVariable (size / 2.0f), 0.0f);
shapeSize = dVector(size, size, size, 0.0f);
collisionArray[i] = CreateConvexCollision (world, dGetIdentityMatrix(), shapeSize, selection[index], materialID);
gemetries[i] = new DemoMesh("geometry", collisionArray[i], "wood_4.tga", "wood_4.tga", "wood_1.tga");
}
// make a large complex of plane by adding lost of these shapes at a random location and oriention;
NewtonCollision* const compound = NewtonCreateCompoundCollision (world, materialID);
NewtonCompoundCollisionBeginAddRemove(compound);
for (int i = 0 ; i < count; i ++) {
for (int j = 0 ; j < count; j ++) {
float pitch = RandomVariable (1.0f) * 2.0f * 3.1416f;
float yaw = RandomVariable (1.0f) * 2.0f * 3.1416f;
float roll = RandomVariable (1.0f) * 2.0f * 3.1416f;
float x = size * (j - count / 2) + RandomVariable (size * 0.5f);
float y = RandomVariable (size * 2.0f);
float z = size * (i - count / 2) + RandomVariable (size * 0.5f);
dMatrix matrix (dPitchMatrix (pitch) * dYawMatrix (yaw) * dRollMatrix (roll));
matrix.m_posit = dVector (x, y, z, 1.0f);
int index = dRand() % (sizeof (selection) / sizeof (selection[0]));
DemoEntity* const entity = new DemoEntity(matrix, this);
entity->SetMesh(gemetries[index], dGetIdentityMatrix());
NewtonCollisionSetMatrix (collisionArray[index], &matrix[0][0]);
NewtonCompoundCollisionAddSubCollision (compound, collisionArray[index]);
}
}
NewtonCompoundCollisionEndAddRemove(compound);
CreateSimpleBody (world, NULL, 0.0f, dGetIdentityMatrix(), compound, 0);
// destroy all collision shapes after they are used
NewtonDestroyCollision(compound);
for (int i = 0; i < int (sizeof (collisionArray) / sizeof (collisionArray[0])); i ++) {
gemetries[i]->Release();
NewtonDestroyCollision(collisionArray[i]);
}
// now make and array of collision shapes for convex casting by mouse point click an drag
CreateCastingShapes(scene, size * 2.0f);
}
void Custom6DOF::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// add the linear limits
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
dVector dp (p0 - p1);
for (int i = 0; i < 3; i ++) {
if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) {
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
} else {
// it is a limited linear dof, check if it pass the limits
dFloat dist = dp.DotProduct3(matrix1[i]);
if (dist > m_maxLinearLimits[i]) {
dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i]));
// clamp the error, so the not too much energy is added when constraint violation occurs
dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
if (maxDist > D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
q1 = p0 - matrix1[i].Scale(D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
}
NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (dist < m_minLinearLimits[i]) {
dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i]));
// clamp the error, so the not too much energy is added when constraint violation occurs
dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
if (maxDist < -D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
q1 = p0 - matrix1[i].Scale(-D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
}
NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
}
dVector euler0(0.0f);
dVector euler1(0.0f);
dMatrix localMatrix (matrix0 * matrix1.Inverse());
localMatrix.GetEulerAngles(euler0, euler1);
AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
euler0 = euler1;
}
dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);
//dTrace (("(%f %f %f) (%f %f %f)\n", m_pitch.m_angle * 180.0f / 3.141592f, m_yaw.m_angle * 180.0f / 3.141592f, m_roll.m_angle * 180.0f / 3.141592f, euler0.m_x * 180.0f / 3.141592f, euler0.m_y * 180.0f / 3.141592f, euler0.m_z * 180.0f / 3.141592f));
bool limitViolation = false;
for (int i = 0; i < 3; i ++) {
if (euler[i] < m_minAngularLimits[i]) {
limitViolation = true;
euler[i] = m_minAngularLimits[i];
} else if (euler[i] > m_maxAngularLimits[i]) {
limitViolation = true;
euler[i] = m_maxAngularLimits[i];
}
}
if (limitViolation) {
//dMatrix pyr (dPitchMatrix(m_pitch.m_angle) * dYawMatrix(m_yaw.m_angle) * dRollMatrix(m_roll.m_angle));
dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
dMatrix baseMatrix (p0y0r0 * matrix1);
dMatrix rotation (matrix0.Inverse() * baseMatrix);
dQuaternion quat (rotation);
if (quat.m_q0 > dFloat (0.99995f)) {
//dVector p0 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
//dVector p1 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &baseMatrix[2][0]);
//NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
//dVector q0 (matrix0[3] + baseMatrix[0].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[1][0]);
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[2][0]);
} else {
dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
dVector q0 (matrix0[3] + basis[1].Scale (MIN_JOINT_PIN_LENGTH));
dVector q1 (matrix0[3] + rotation.RotateVector(basis[1].Scale (MIN_JOINT_PIN_LENGTH)));
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &basis[2][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
//dVector q0 (matrix0[3] + basis[0].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[1][0]);
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[2][0]);
}
}
}
void RenderJointsDebugInfo (NewtonWorld* const world, dFloat size)
{
glDisable(GL_TEXTURE_2D);
glDisable (GL_LIGHTING);
glBegin(GL_LINES);
// this will go over the joint list twice,
for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) {
for (NewtonJoint* joint = NewtonBodyGetFirstJoint(body); joint; joint = NewtonBodyGetNextJoint(body, joint)) {
NewtonJointRecord info;
NewtonJointGetInfo (joint, &info);
if (strcmp (info.m_descriptionType, "customJointNotInfo")) {
// draw first frame
dMatrix matrix0;
NewtonBodyGetMatrix (info.m_attachBody_0, &matrix0[0][0]);
matrix0 = dMatrix (&info.m_attachmenMatrix_0[0][0]) * matrix0;
dVector o0 (matrix0.m_posit);
dVector x (o0 + matrix0.RotateVector (dVector (size, 0.0f, 0.0f, 0.0f)));
glColor3f (1.0f, 0.0f, 0.0f);
glVertex3f (o0.m_x, o0.m_y, o0.m_z);
glVertex3f (x.m_x, x.m_y, x.m_z);
dVector y (o0 + matrix0.RotateVector (dVector (0.0f, size, 0.0f, 0.0f)));
glColor3f (0.0f, 1.0f, 0.0f);
glVertex3f (o0.m_x, o0.m_y, o0.m_z);
glVertex3f (y.m_x, y.m_y, y.m_z);
dVector z (o0 + matrix0.RotateVector (dVector (0.0f, 0.0f, size, 0.0f)));
glColor3f (0.0f, 0.0f, 1.0f);
glVertex3f (o0.m_x, o0.m_y, o0.m_z);
glVertex3f (z.m_x, z.m_y, z.m_z);
// draw second frame
dMatrix matrix1 (dGetIdentityMatrix());
if (info.m_attachBody_1) {
NewtonBodyGetMatrix (info.m_attachBody_1, &matrix1[0][0]);
}
matrix1 = dMatrix (&info.m_attachmenMatrix_1[0][0]) * matrix1;
dVector o1 (matrix1.m_posit);
x = o1 + matrix1.RotateVector (dVector (size, 0.0f, 0.0f, 0.0f));
glColor3f (1.0f, 0.0f, 0.0f);
glVertex3f (o1.m_x, o1.m_y, o1.m_z);
glVertex3f (x.m_x, x.m_y, x.m_z);
y = o1 + matrix1.RotateVector (dVector (0.0f, size, 0.0f, 0.0f));
glColor3f (0.0f, 1.0f, 0.0f);
glVertex3f (o1.m_x, o1.m_y, o1.m_z);
glVertex3f (y.m_x, y.m_y, y.m_z);
z = o1 + matrix1.RotateVector (dVector (0.0f, 0.0f, size, 0.0f));
glColor3f (0.0f, 0.0f, 1.0f);
glVertex3f (o1.m_x, o1.m_y, o1.m_z);
glVertex3f (z.m_x, z.m_y, z.m_z);
if (!strcmp (info.m_descriptionType, "limitballsocket")) {
// draw the cone limit of this joint
int steps = 12;
dMatrix coneMatrix (dRollMatrix(info.m_maxAngularDof[1]));
dMatrix ratationStep (dPitchMatrix(2.0f * 3.14151693f / steps));
dVector p0 (coneMatrix.RotateVector(dVector (size * 0.5f, 0.0f, 0.0f, 0.0f)));
dVector q0 (matrix1.TransformVector(p0));
glColor3f (1.0f, 1.0f, 0.0f);
for (int i = 0; i < (steps + 1); i ++) {
dVector p1 (ratationStep.RotateVector(p0));
dVector q1 (matrix1.TransformVector(p1));
glVertex3f (o0.m_x, o0.m_y, o0.m_z);
glVertex3f (q0.m_x, q0.m_y, q0.m_z);
glVertex3f (q0.m_x, q0.m_y, q0.m_z);
glVertex3f (q1.m_x, q1.m_y, q1.m_z);
p0 = p1;
q0 = q1;
}
}
}
}
}
glEnd();
}
void dSceneRender::DrawCone(int segments, dFloat radius, dFloat heigh)
{
dVector q1 (-heigh / 2.0f, radius, 0.0f, 0.0f);
dMatrix rotation (dPitchMatrix(2.0f * 3.1614f/segments));
dVector cap[1024];
dAssert (segments < sizeof (cap)/sizeof (cap[0]));
cap[segments] = q1;
for (int i = 0; i < segments; i ++) {
cap[i] = q1;
q1 = rotation.RotateVector(q1);
}
dVector normal1 (-1.0f, 0.0f, 0.0f, 0.0f);
BeginTriangle();
for (int i = 2; i < segments; i ++) {
SubmitNormal(normal1);
DrawTriangle(cap[0], cap[i], cap[i - 1]);
}
dVector p0 ( heigh / 2.0f, 0.0f, 0.0f, 0.0f);
for (int i = 0; i < segments; i ++) {
dVector p1 (cap[i]);
dVector p2 (cap[i + 1]);
dVector normal ((p1 - p0) * (p2 - p0));
normal = normal.Scale (1.0f / dSqrt(normal % normal));
SubmitNormal(normal);
DrawTriangle(p0, p1, p2);
}
End();
/*
{
dVector p1 (-heigh / 2.0f, radius, 0.0f, 0.0f);
dMatrix rotation (dPitchMatrix(2.0f * 3.1614f/segments));
dVector cap[1024];
dAssert (segments < sizeof (cap)/sizeof (cap[0]));
cap[segments] = p1;
for (int i = 0; i < segments; i ++) {
cap[i] = p1;
p1 = rotation.RotateVector(p1);
}
dVector normal1 (-1.0f, 0.0f, 0.0f, 0.0f);
BeginLine();
DrawLine(cap[0], cap[0] + normal1);
dVector p0 ( heigh / 2.0f, 0.0f, 0.0f, 0.0f);
for (int i = 0; i < segments; i ++) {
dVector p1 (cap[i]);
dVector p2 (cap[i + 1]);
dVector normal ((p1 - p0) * (p2 - p0));
normal = normal.Scale (1.0f / dSqrt(normal % normal));
DrawLine(p0, p0 + normal);
}
End();
}
*/
}
// create physics scene
void PuckSlide (DemoEntityManager* const scene)
{
scene->CreateSkyBox();
NewtonWorld* const world = scene->GetNewton();
int materialGroupIDs[SB_NUM_MATERIALS];
// Create groups
for (int i = 0; i < SB_NUM_MATERIALS; i++)
{
materialGroupIDs[i] = NewtonMaterialCreateGroupID(world);
}
// Setup the material data
NewtonMaterialSetDefaultSoftness(world, materialGroupIDs[SBMaterial_WEIGHT], materialGroupIDs[SBMaterial_SURFACE], 0.15f);
NewtonMaterialSetDefaultElasticity(world, materialGroupIDs[SBMaterial_WEIGHT], materialGroupIDs[SBMaterial_SURFACE], 0.30f);
NewtonMaterialSetDefaultFriction(world, materialGroupIDs[SBMaterial_WEIGHT], materialGroupIDs[SBMaterial_SURFACE], 0.05f, 0.04f);
// setup callbacks for collisions between two material groups
NewtonMaterialSetCollisionCallback(world,materialGroupIDs[SBMaterial_WEIGHT],materialGroupIDs[SBMaterial_SURFACE],NULL,PhysicsNewton_CollisionPuckSurfaceCB);
///////
// Add table
{
dVector tableSize(TABLE_LENGTH, TABLE_HEIGHT, TABLE_WIDTH, 0.0f);
// create the shape and visual mesh as a common data to be re used
NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), tableSize, _BOX_PRIMITIVE, materialGroupIDs[SBMaterial_SURFACE]);
DemoMesh* const geometry = new DemoMesh("cylinder_1", collision, "wood_3.tga", "wood_3.tga", "wood_3.tga");
dMatrix matrix = dGetIdentityMatrix();
matrix.m_posit.m_x = 0.0f;
matrix.m_posit.m_z = 0.0f;
matrix.m_posit.m_y = 0.0f;
NewtonBody* const tableBody = CreateSimpleSolid (scene, geometry, 0.0, matrix, collision, materialGroupIDs[SBMaterial_SURFACE]);
// this is deprecated, use NewtonBodySetMassProperties
//NewtonBodySetMassMatrix(tableBody, 0.0f, 1.0f, 1.0f, 1.0f);
NewtonBodySetMassProperties(tableBody, 0.0f, NewtonBodyGetCollision(tableBody));
NewtonBodySetMaterialGroupID(tableBody, materialGroupIDs[SBMaterial_SURFACE]);
// it is not wise to se static body to continuous collision mode
//NewtonBodySetContinuousCollisionMode(tableBody, 1);
// do not forget to release the assets
geometry->Release();
// the collision need to be destroy, the body is using an instance no a reference
NewtonDestroyCollision (collision);
}
///////
// Add puck
{
new PuckEntity (scene, materialGroupIDs[SBMaterial_WEIGHT]);
}
// place camera into position
dMatrix camMatrix (dPitchMatrix(20.0f * 3.1416f /180.0f));
dQuaternion rot (camMatrix);
dVector origin (CAMERA_Z, CAMERA_Y, CAMERA_X, 0.0f);
scene->SetCameraMatrix(rot, origin);
}
NewtonCollision* CreateConvexCollision (NewtonWorld* world, const dMatrix& srcMatrix, const dVector& originalSize, PrimitiveType type, int materialID__)
{
dVector size (originalSize);
NewtonCollision* collision = NULL;
switch (type)
{
case _NULL_PRIMITIVE:
{
collision = NewtonCreateNull (world);
break;
}
case _SPHERE_PRIMITIVE:
{
// create the collision
collision = NewtonCreateSphere (world, size.m_x * 0.5f, 0, NULL);
break;
}
case _BOX_PRIMITIVE:
{
// create the collision
collision = NewtonCreateBox (world, size.m_x, size.m_y, size.m_z, 0, NULL);
break;
}
case _CONE_PRIMITIVE:
{
dFloat r = size.m_x * 0.5f;
dFloat h = size.m_y;
// create the collision
collision = NewtonCreateCone (world, r, h, 0, NULL);
break;
}
case _CYLINDER_PRIMITIVE:
{
// create the collision
collision = NewtonCreateCylinder (world, size.m_x * 0.5f, size.m_y, 0, NULL);
break;
}
case _CAPSULE_PRIMITIVE:
{
// create the collision
collision = NewtonCreateCapsule (world, size.m_x * 0.5f, size.m_y, 0, NULL);
break;
}
case _TAPERED_CAPSULE_PRIMITIVE:
{
// create the collision
collision = NewtonCreateTaperedCapsule (world, size.m_x * 0.5f, size.m_z * 0.5f, size.m_y, 0, NULL);
break;
}
case _CHAMFER_CYLINDER_PRIMITIVE:
{
// create the collision
collision = NewtonCreateChamferCylinder (world, size.m_x * 0.5f, size.m_y, 0, NULL);
break;
}
case _TAPERED_CYLINDER_PRIMITIVE:
{
// create the collision
collision = NewtonCreateTaperedCylinder (world, size.m_x * 0.5f, size.m_z * 0.5f, size.m_y, 0, NULL);
break;
}
case _RANDOM_CONVEX_HULL_PRIMITIVE:
{
// Create a clouds of random point around the origin
#define SAMPLE_COUNT 200
dVector cloud [SAMPLE_COUNT];
// make sure that at least the top and bottom are present
cloud [0] = dVector ( size.m_x * 0.5f, 0.0f, 0.0f, 0.0f);
cloud [1] = dVector (-size.m_x * 0.5f, 0.0f, 0.0f, 0.0f);
cloud [2] = dVector ( 0.0f, size.m_y * 0.5f, 0.0f, 0.0f);
cloud [3] = dVector ( 0.0f, -size.m_y * 0.5f, 0.0f, 0.0f);
cloud [4] = dVector (0.0f, 0.0f, size.m_z * 0.5f, 0.0f);
cloud [5] = dVector (0.0f, 0.0f, -size.m_z * 0.5f, 0.0f);
int count = 6;
// populate the cloud with pseudo Gaussian random points
for (int i = 6; i < SAMPLE_COUNT; i ++) {
cloud [i].m_x = RandomVariable(size.m_x);
cloud [i].m_y = RandomVariable(size.m_y);
cloud [i].m_z = RandomVariable(size.m_z);
count ++;
}
collision = NewtonCreateConvexHull (world, count, &cloud[0].m_x, sizeof (dVector), 0.01f, 0, NULL);
break;
}
case _REGULAR_CONVEX_HULL_PRIMITIVE:
{
// Create a clouds of random point around the origin
#define STEPS_HULL 6
//#define STEPS_HULL 3
dVector cloud [STEPS_HULL * 4 + 256];
int count = 0;
dFloat radius = size.m_y;
dFloat height = size.m_x * 0.999f;
dFloat x = - height * 0.5f;
dMatrix rotation (dPitchMatrix(2.0f * 3.141592f / STEPS_HULL));
for (int i = 0; i < 4; i ++) {
dFloat pad = ((i == 1) || (i == 2)) * 0.25f * radius;
dVector p (x, 0.0f, radius + pad);
x += 0.3333f * height;
dMatrix acc (dGetIdentityMatrix());
for (int j = 0; j < STEPS_HULL; j ++) {
cloud[count] = acc.RotateVector(p);
acc = acc * rotation;
count ++;
}
}
collision = NewtonCreateConvexHull (world, count, &cloud[0].m_x, sizeof (dVector), 0.02f, 0, NULL);
break;
}
case _COMPOUND_CONVEX_CRUZ_PRIMITIVE:
{
//dMatrix matrix (GetIdentityMatrix());
dMatrix matrix (dPitchMatrix(15.0f * 3.1416f / 180.0f) * dYawMatrix(15.0f * 3.1416f / 180.0f) * dRollMatrix(15.0f * 3.1416f / 180.0f));
// NewtonCollision* const collisionA = NewtonCreateBox (world, size.m_x, size.m_x * 0.25f, size.m_x * 0.25f, 0, &matrix[0][0]);
// NewtonCollision* const collisionB = NewtonCreateBox (world, size.m_x * 0.25f, size.m_x, size.m_x * 0.25f, 0, &matrix[0][0]);
// NewtonCollision* const collisionC = NewtonCreateBox (world, size.m_x * 0.25f, size.m_x * 0.25f, size.m_x, 0, &matrix[0][0]);
matrix.m_posit = dVector (size.m_x * 0.5f, 0.0f, 0.0f, 1.0f);
NewtonCollision* const collisionA = NewtonCreateBox (world, size.m_x, size.m_x * 0.25f, size.m_x * 0.25f, 0, &matrix[0][0]);
matrix.m_posit = dVector (0.0f, size.m_x * 0.5f, 0.0f, 1.0f);
NewtonCollision* const collisionB = NewtonCreateBox (world, size.m_x * 0.25f, size.m_x, size.m_x * 0.25f, 0, &matrix[0][0]);
matrix.m_posit = dVector (0.0f, 0.0f, size.m_x * 0.5f, 1.0f);
NewtonCollision* const collisionC = NewtonCreateBox (world, size.m_x * 0.25f, size.m_x * 0.25f, size.m_x, 0, &matrix[0][0]);
collision = NewtonCreateCompoundCollision (world, 0);
NewtonCompoundCollisionBeginAddRemove(collision);
NewtonCompoundCollisionAddSubCollision (collision, collisionA);
NewtonCompoundCollisionAddSubCollision (collision, collisionB);
NewtonCompoundCollisionAddSubCollision (collision, collisionC);
NewtonCompoundCollisionEndAddRemove(collision);
NewtonDestroyCollision(collisionA);
NewtonDestroyCollision(collisionB);
NewtonDestroyCollision(collisionC);
break;
}
default: dAssert (0);
}
dMatrix matrix (srcMatrix);
matrix.m_front = matrix.m_front.Scale (1.0f / dSqrt (matrix.m_front % matrix.m_front));
matrix.m_right = matrix.m_front * matrix.m_up;
matrix.m_right = matrix.m_right.Scale (1.0f / dSqrt (matrix.m_right % matrix.m_right));
matrix.m_up = matrix.m_right * matrix.m_front;
NewtonCollisionSetMatrix(collision, &matrix[0][0]);
return collision;
}
static void MakeFunnyCompound (DemoEntityManager* const scene, const dVector& origin)
{
NewtonWorld* const world = scene->GetNewton();
// create an empty compound collision
NewtonCollision* const compound = NewtonCreateCompoundCollision (world, 0);
#if 1
NewtonCompoundCollisionBeginAddRemove(compound);
// add a bunch of convex collision at random position and orientation over the surface of a big sphere
float radio = 5.0f;
for (int i = 0 ; i < 300; i ++) {
NewtonCollision* collision = NULL;
float pitch = RandomVariable (1.0f) * 2.0f * 3.1416f;
float yaw = RandomVariable (1.0f) * 2.0f * 3.1416f;
float roll = RandomVariable (1.0f) * 2.0f * 3.1416f;
float x = RandomVariable (0.5f);
float y = RandomVariable (0.5f);
float z = RandomVariable (0.5f);
if ((x == 0.0f) && (y == 0.0f) && (z == 0.0f)){
x = 0.1f;
}
dVector p (x, y, z, 1.0f) ;
p = p.Scale (radio / dSqrt (p % p));
dMatrix matrix (dPitchMatrix (pitch) * dYawMatrix (yaw) * dRollMatrix (roll));
matrix.m_posit = p;
int r = dRand();
switch ((r >>2) & 3)
{
case 0:
{
collision = NewtonCreateSphere(world, 0.5, 0, &matrix[0][0]) ;
break;
}
case 1:
{
collision = NewtonCreateCapsule(world, 0.3f, 0.2f, 0.5f, 0, &matrix[0][0]) ;
break;
}
case 2:
{
collision = NewtonCreateCylinder(world, 0.25, 0.5, 0.25, 0, &matrix[0][0]) ;
break;
}
case 3:
{
collision = NewtonCreateCone(world, 0.25, 0.25, 0, &matrix[0][0]) ;
break;
}
}
dAssert (collision);
// we can set a collision id, and use data per sub collision
NewtonCollisionSetUserID(collision, i);
NewtonCollisionSetUserData(collision, (void*) i);
// add this new collision
NewtonCompoundCollisionAddSubCollision (compound, collision);
NewtonDestroyCollision(collision);
}
// finish adding shapes
NewtonCompoundCollisionEndAddRemove(compound);
{
// remove the first 10 shapes
// test remove shape form a compound
NewtonCompoundCollisionBeginAddRemove(compound);
void* node = NewtonCompoundCollisionGetFirstNode(compound);
for (int i = 0; i < 10; i ++) {
//NewtonCollision* const collision = NewtonCompoundCollisionGetCollisionFromNode(compound, node);
void* const nextNode = NewtonCompoundCollisionGetNextNode(compound, node);
NewtonCompoundCollisionRemoveSubCollision(compound, node);
node = nextNode;
}
// finish remove
void* handle1 = NewtonCompoundCollisionGetNodeByIndex (compound, 30);
void* handle2 = NewtonCompoundCollisionGetNodeByIndex (compound, 100);
NewtonCollision* const shape1 = NewtonCompoundCollisionGetCollisionFromNode (compound, handle1);
NewtonCollision* const shape2 = NewtonCompoundCollisionGetCollisionFromNode (compound, handle2);
NewtonCollision* const copyShape1 = NewtonCollisionCreateInstance (shape1);
NewtonCollision* const copyShape2 = NewtonCollisionCreateInstance (shape2);
// you can also remove shape by their index
NewtonCompoundCollisionRemoveSubCollisionByIndex (compound, 30);
NewtonCompoundCollisionRemoveSubCollisionByIndex (compound, 100);
handle1 = NewtonCompoundCollisionAddSubCollision (compound, copyShape1);
handle2 = NewtonCompoundCollisionAddSubCollision (compound, copyShape2);
NewtonDestroyCollision(copyShape1);
NewtonDestroyCollision(copyShape2);
NewtonCompoundCollisionEndAddRemove(compound);
}
{
// show how to modify the children of a compound collision
NewtonCompoundCollisionBeginAddRemove(compound);
for (void* node = NewtonCompoundCollisionGetFirstNode(compound); node; node = NewtonCompoundCollisionGetNextNode(compound, node)) {
NewtonCollision* const collision = NewtonCompoundCollisionGetCollisionFromNode(compound, node);
// you can scale, change the matrix, change the inertia, do anything you want with the change
NewtonCollisionSetUserData(collision, NULL);
}
NewtonCompoundCollisionEndAddRemove(compound);
}
// NewtonCollisionSetScale(compound, 0.5f, 0.25f, 0.125f);
#else
//test Yeside compound shape shape
// - Rotation="1.5708 -0 0" Translation="0 0 0.024399" Size="0.021 0.096" Pos="0 0 0.115947"
// - Rotation="1.5708 -0 0" Translation="0 0 0.056366" Size="0.195 0.024" Pos="0 0 0.147914"
// - Rotation="1.5708 -0 0" Translation="0 0 -0.056366" Size="0.0065 0.07 Pos="0 0 0.035182"
NewtonCompoundCollisionBeginAddRemove(compound);
NewtonCollision* collision;
dMatrix offsetMatrix (dPitchMatrix(1.5708f));
offsetMatrix.m_posit.m_z = 0.115947f;
collision = NewtonCreateCylinder (world, 0.021f, 0.096f, 0, &offsetMatrix[0][0]) ;
NewtonCompoundCollisionAddSubCollision (compound, collision);
NewtonDestroyCollision(collision);
offsetMatrix.m_posit.m_z = 0.035182f;
collision = NewtonCreateCylinder (world, 0.0065f, 0.07f, 0, &offsetMatrix[0][0]) ;
NewtonCompoundCollisionAddSubCollision (compound, collision);
NewtonDestroyCollision(collision);
offsetMatrix.m_posit.m_z = 0.147914f;
collision = NewtonCreateCylinder (world, 0.195f, 0.024f, 0, &offsetMatrix[0][0]) ;
NewtonCompoundCollisionAddSubCollision (compound, collision);
NewtonDestroyCollision(collision);
NewtonCompoundCollisionEndAddRemove(compound);
#endif
// for now we will simple make simple Box, make a visual Mesh
DemoMesh* const visualMesh = new DemoMesh ("big ball", compound, "metal_30.tga", "metal_30.tga", "metal_30.tga");
int instaceCount = 2;
dMatrix matrix (dGetIdentityMatrix());
matrix.m_posit = origin;
for (int ix = 0; ix < instaceCount; ix ++) {
for (int iz = 0; iz < instaceCount; iz ++) {
dFloat y = origin.m_y;
dFloat x = origin.m_x + (ix - instaceCount/2) * 15.0f;
dFloat z = origin.m_z + (iz - instaceCount/2) * 15.0f;
matrix.m_posit = FindFloor (world, dVector (x, y + 10.0f, z, 0.0f), 20.0f); ;
matrix.m_posit.m_y += 15.0f;
CreateSimpleSolid (scene, visualMesh, 10.0f, matrix, compound, 0);
}
}
visualMesh->Release();
NewtonDestroyCollision(compound);
}
void dCustomBallAndSocket::Debug(dDebugDisplay* const debugDisplay) const
{
dMatrix matrix0;
dMatrix matrix1;
dCustomJoint::Debug(debugDisplay);
CalculateGlobalMatrix(matrix0, matrix1);
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngleCos = coneDir0.DotProduct3(coneDir1);
dMatrix coneRotation(dGetIdentityMatrix());
if (cosAngleCos < 0.9999f) {
dVector lateralDir(coneDir1.CrossProduct(coneDir0));
dFloat mag2 = lateralDir.DotProduct3(lateralDir);
//dAssert(mag2 > 1.0e-4f);
if (mag2 > 1.0e-4f) {
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit);
} else {
lateralDir = matrix0.m_up.Scale(-1.0f);
coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit);
}
} else if (cosAngleCos < -0.9999f) {
coneRotation[0][0] = -1.0f;
coneRotation[1][1] = -1.0f;
}
const int subdiv = 18;
const dFloat radius = debugDisplay->m_debugScale;
dVector arch[subdiv + 1];
// show twist angle limits
if (m_options.m_option0 && ((m_maxTwistAngle - m_minTwistAngle) > dFloat(1.0e-3f))) {
dMatrix pitchMatrix(matrix1 * coneRotation);
pitchMatrix.m_posit = matrix1.m_posit;
dVector point(dFloat(0.0f), dFloat(radius), dFloat(0.0f), dFloat(0.0f));
dFloat angleStep = dMin (m_maxTwistAngle - m_minTwistAngle, dFloat (2.0f * dPi)) / subdiv;
dFloat angle0 = m_minTwistAngle;
debugDisplay->SetColor(dVector(0.6f, 0.2f, 0.0f, 0.0f));
for (int i = 0; i <= subdiv; i++) {
arch[i] = pitchMatrix.TransformVector(dPitchMatrix(angle0).RotateVector(point));
debugDisplay->DrawLine(pitchMatrix.m_posit, arch[i]);
angle0 += angleStep;
}
for (int i = 0; i < subdiv; i++) {
debugDisplay->DrawLine(arch[i], arch[i + 1]);
}
}
// show cone angle limits
if (m_options.m_option2) {
dVector point(radius * dCos(m_maxConeAngle), radius * dSin(m_maxConeAngle), 0.0f, 0.0f);
dFloat angleStep = dPi * 2.0f / subdiv;
dFloat angle0 = 0.0f;
debugDisplay->SetColor(dVector(0.3f, 0.8f, 0.0f, 0.0f));
for (int i = 0; i <= subdiv; i++) {
dVector conePoint(dPitchMatrix(angle0).RotateVector(point));
dVector p(matrix1.TransformVector(conePoint));
arch[i] = p;
debugDisplay->DrawLine(matrix1.m_posit, p);
angle0 += angleStep;
}
for (int i = 0; i < subdiv; i++) {
debugDisplay->DrawLine(arch[i], arch[i + 1]);
}
}
}
void CustomPlayerController::Init(dFloat mass, dFloat outerRadius, dFloat innerRadius, dFloat height, dFloat stairStep, const dMatrix& localAxis)
{
dAssert (stairStep >= 0.0f);
dAssert (innerRadius >= 0.0f);
dAssert (outerRadius >= innerRadius);
dAssert (height >= stairStep);
dAssert (localAxis[0].m_w == dFloat (0.0f));
dAssert (localAxis[1].m_w == dFloat (0.0f));
CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager();
NewtonWorld* const world = manager->GetWorld();
SetRestrainingDistance (0.0f);
m_outerRadio = outerRadius;
m_innerRadio = innerRadius;
m_height = height;
m_stairStep = stairStep;
SetClimbSlope(45.0f * 3.1416f/ 180.0f);
m_upVector = localAxis[0];
m_frontVector = localAxis[1];
m_groundPlane = dVector (0.0f, 0.0f, 0.0f, 0.0f);
m_groundVelocity = dVector (0.0f, 0.0f, 0.0f, 0.0f);
const int steps = 12;
dVector convexPoints[2][steps];
// create an inner thin cylinder
dFloat shapeHigh = height;
dAssert (shapeHigh > 0.0f);
dVector p0 (0.0f, m_innerRadio, 0.0f, 0.0f);
dVector p1 (shapeHigh, m_innerRadio, 0.0f, 0.0f);
for (int i = 0; i < steps; i ++) {
dMatrix rotation (dPitchMatrix (i * 2.0f * 3.141592f / steps));
convexPoints[0][i] = localAxis.RotateVector(rotation.RotateVector(p0));
convexPoints[1][i] = localAxis.RotateVector(rotation.RotateVector(p1));
}
NewtonCollision* const supportShape = NewtonCreateConvexHull(world, steps * 2, &convexPoints[0][0].m_x, sizeof (dVector), 0.0f, 0, NULL);
// create the outer thick cylinder
dMatrix outerShapeMatrix (localAxis);
dFloat capsuleHigh = m_height - stairStep;
dAssert (capsuleHigh > 0.0f);
m_sphereCastOrigin = capsuleHigh * 0.5f + stairStep;
outerShapeMatrix.m_posit = outerShapeMatrix[0].Scale(m_sphereCastOrigin);
outerShapeMatrix.m_posit.m_w = 1.0f;
NewtonCollision* const bodyCapsule = NewtonCreateCapsule(world, 0.25f, 0.5f, 0, &outerShapeMatrix[0][0]);
NewtonCollisionSetScale(bodyCapsule, capsuleHigh, m_outerRadio * 4.0f, m_outerRadio * 4.0f);
// compound collision player controller
NewtonCollision* const playerShape = NewtonCreateCompoundCollision(world, 0);
NewtonCompoundCollisionBeginAddRemove(playerShape);
NewtonCompoundCollisionAddSubCollision (playerShape, supportShape);
NewtonCompoundCollisionAddSubCollision (playerShape, bodyCapsule);
NewtonCompoundCollisionEndAddRemove (playerShape);
// create the kinematic body
dMatrix locationMatrix (dGetIdentityMatrix());
m_body = NewtonCreateKinematicBody(world, playerShape, &locationMatrix[0][0]);
// players must have weight, otherwise they are infinitely strong when they collide
NewtonCollision* const shape = NewtonBodyGetCollision(m_body);
NewtonBodySetMassProperties(m_body, mass, shape);
// make the body collidable with other dynamics bodies, by default
NewtonBodySetCollidable (m_body, true);
dFloat castHigh = capsuleHigh * 0.4f;
dFloat castRadio = (m_innerRadio * 0.5f > 0.05f) ? m_innerRadio * 0.5f : 0.05f;
dVector q0 (0.0f, castRadio, 0.0f, 0.0f);
dVector q1 (castHigh, castRadio, 0.0f, 0.0f);
for (int i = 0; i < steps; i ++) {
dMatrix rotation (dPitchMatrix (i * 2.0f * 3.141592f / steps));
convexPoints[0][i] = localAxis.RotateVector(rotation.RotateVector(q0));
convexPoints[1][i] = localAxis.RotateVector(rotation.RotateVector(q1));
}
m_castingShape = NewtonCreateConvexHull(world, steps * 2, &convexPoints[0][0].m_x, sizeof (dVector), 0.0f, 0, NULL);
m_supportShape = NewtonCompoundCollisionGetCollisionFromNode (shape, NewtonCompoundCollisionGetNodeByIndex (shape, 0));
m_upperBodyShape = NewtonCompoundCollisionGetCollisionFromNode (shape, NewtonCompoundCollisionGetNodeByIndex (shape, 1));
NewtonDestroyCollision (bodyCapsule);
NewtonDestroyCollision (supportShape);
NewtonDestroyCollision (playerShape);
m_isJumping = false;
}
void CustomControlledBallAndSocket::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
#if 0
dVector euler0;
dVector euler1;
dMatrix localMatrix (matrix0 * matrix1.Inverse());
localMatrix.GetEulerAngles(euler0, euler1);
AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
euler0 = euler1;
}
dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);
for (int i = 0; i < 3; i ++) {
dFloat error = m_targetAngles[i] - euler[i];
if (dAbs (error) > (0.125f * 3.14159213f / 180.0f) ) {
dFloat angularStep = dSign(error) * m_angulaSpeed * timestep;
if (angularStep > 0.0f) {
if (angularStep > error) {
angularStep = error * 0.5f;
}
} else {
if (angularStep < error) {
angularStep = error * 0.5f;
}
}
euler[i] = euler[i] + angularStep;
}
}
dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
dMatrix baseMatrix (p0y0r0 * matrix1);
dMatrix rotation (matrix0.Inverse() * baseMatrix);
dQuaternion quat (rotation);
if (quat.m_q0 > dFloat (0.99995f)) {
dVector euler0;
dVector euler1;
rotation.GetEulerAngles(euler0, euler1);
NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]);
} else {
dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
NewtonUserJointAddAngularRow (m_joint, 2.0f * dAcos (quat.m_q0), &basis[0][0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[1][0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[2][0]);
}
#else
matrix1 = m_targetRotation * matrix1;
dQuaternion localRotation(matrix1 * matrix0.Inverse());
if (localRotation.DotProduct(m_targetRotation) < 0.0f) {
localRotation.Scale(-1.0f);
}
dFloat angle = 2.0f * dAcos(localRotation.m_q0);
dFloat angleStep = m_angulaSpeed * timestep;
if (angleStep < angle) {
dVector axis(dVector(localRotation.m_q1, localRotation.m_q2, localRotation.m_q3, 0.0f));
axis = axis.Scale(1.0f / dSqrt(axis % axis));
// localRotation = dQuaternion(axis, angleStep);
}
dVector axis (matrix1.m_front * matrix1.m_front);
dVector axis1 (matrix1.m_front * matrix1.m_front);
//dFloat sinAngle;
//dFloat cosAngle;
//CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle);
//float xxxx = dAtan2(sinAngle, cosAngle);
//float xxxx1 = dAtan2(sinAngle, cosAngle);
dQuaternion quat(localRotation);
if (quat.m_q0 > dFloat(0.99995f)) {
// dAssert (0);
/*
dVector euler0;
dVector euler1;
rotation.GetEulerAngles(euler0, euler1);
NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]);
*/
} else {
dMatrix basis(dGrammSchmidt(dVector(quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
NewtonUserJointAddAngularRow(m_joint, -2.0f * dAcos(quat.m_q0), &basis[0][0]);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]);
}
#endif
}
void CustomControlledBallAndSocket::UpdateTargetMatrix ()
{
m_targetRotation = dPitchMatrix(m_targetAngles[0]) * dYawMatrix(m_targetAngles[1]) * dRollMatrix(m_targetAngles[2]);
}
