API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Oct 31st, 2016
168
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 43.92 KB | None | 0 0
raw download clone embed print report
  1. /* Copyright (c) <2003-2016> <Newton Game Dynamics>
  2. *
  3. * This software is provided 'as-is', without any express or implied
  4. * warranty. In no event will the authors be held liable for any damages
  5. * arising from the use of this software.
  6. *
  7. * Permission is granted to anyone to use this software for any purpose,
  8. * including commercial applications, and to alter it and redistribute it
  9. * freely
  10. */
  11.  
  12. #include <toolbox_stdafx.h>
  13. #include "SkyBox.h"
  14. #include "DemoEntityManager.h"
  15. #include "DemoCamera.h"
  16. #include "PhysicsUtils.h"
  17. #include "DemoMesh.h"
  18. #include "OpenGlUtil.h"
  19. #include <CustomGear.h>
  20. #include <Custom6DOF.h>
  21. #include <CustomHinge.h>
  22. #include <CustomSlider.h>
  23. #include <CustomPulley.h>
  24. #include <dBezierSpline.h>
  25. #include <CustomCorkScrew.h>
  26. #include <CustomPathFollow.h>
  27. #include <CustomBallAndSocket.h>
  28. #include <CustomRackAndPinion.h>
  29. #include <CustomSlidingContact.h>
  30.  
  31. // optionally uncomment this for hard joint simulations
  32. #define _USE_HARD_JOINTS
  33.  
  34.  
  35. static NewtonBody* CreateBox (DemoEntityManager* const scene, const dVector& location, const dVector& size)
  36. {
  37.     NewtonWorld* const world = scene->GetNewton();
  38.     int materialID =  NewtonMaterialGetDefaultGroupID (world);
  39.     NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, _BOX_PRIMITIVE, 0);
  40.     DemoMesh* const geometry = new DemoMesh("primitive", collision, "smilli.tga", "smilli.tga", "smilli.tga");
  41.  
  42.     dFloat mass = 1.0f;
  43.     dMatrix matrix (dGetIdentityMatrix());
  44.     matrix.m_posit = location;
  45.     matrix.m_posit.m_w = 1.0f;
  46.     NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);
  47.  
  48.     geometry->Release();
  49.     NewtonDestroyCollision(collision);
  50.     return body;
  51. }
  52.  
  53. static NewtonBody* CreateCapule (DemoEntityManager* const scene, const dVector& location, const dVector& size)
  54. {
  55.     NewtonWorld* const world = scene->GetNewton();
  56.     int materialID =  NewtonMaterialGetDefaultGroupID (world);
  57.     dMatrix uprightAligment (dRollMatrix(3.141592f * 90.0f / 180.0f));
  58.     NewtonCollision* const collision = CreateConvexCollision (world, &uprightAligment[0][0], size, _CAPSULE_PRIMITIVE, 0);
  59.     DemoMesh* const geometry = new DemoMesh("primitive", collision, "smilli.tga", "smilli.tga", "smilli.tga");
  60.  
  61.     dFloat mass = 1.0f;
  62.     dMatrix matrix (dGetIdentityMatrix());
  63.     matrix.m_posit = location;
  64.     matrix.m_posit.m_w = 1.0f;
  65.     NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);
  66.  
  67.     geometry->Release();
  68.     NewtonDestroyCollision(collision);
  69.     return body;
  70. }
  71.  
  72. static NewtonBody* CreateWheel (DemoEntityManager* const scene, const dVector& location, dFloat radius, dFloat height)
  73. {
  74.     NewtonWorld* const world = scene->GetNewton();
  75.     int materialID =  NewtonMaterialGetDefaultGroupID (world);
  76.     dVector size (radius, height, radius, 0.0f);
  77.     NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, _CHAMFER_CYLINDER_PRIMITIVE, 0);
  78.     DemoMesh* const geometry = new DemoMesh("primitive", collision, "smilli.tga", "smilli.tga", "smilli.tga");
  79.  
  80.     dFloat mass = 1.0f;
  81.     dMatrix matrix (dGetIdentityMatrix());
  82.     matrix.m_posit = location;
  83.     matrix.m_posit.m_w = 1.0f;
  84.     NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);
  85.  
  86.     geometry->Release();
  87.     NewtonDestroyCollision(collision);
  88.     return body;
  89. }
  90.  
  91. static NewtonBody* CreateCylinder (DemoEntityManager* const scene, const dVector& location, dFloat radius, dFloat height)
  92. {
  93.     NewtonWorld* const world = scene->GetNewton();
  94.     int materialID =  NewtonMaterialGetDefaultGroupID (world);
  95.     dVector size (radius, height, radius, 0.0f);
  96.     NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, _CYLINDER_PRIMITIVE, 0);
  97.     DemoMesh* const geometry = new DemoMesh("primitive", collision, "smilli.tga", "smilli.tga", "smilli.tga");
  98.  
  99.     dFloat mass = 1.0f;
  100.     dMatrix matrix (dGetIdentityMatrix());
  101.     matrix.m_posit = location;
  102.     matrix.m_posit.m_w = 1.0f;
  103.     NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);
  104.  
  105.     geometry->Release();
  106.     NewtonDestroyCollision(collision);
  107.     return body;
  108. }
  109.  
  110. static void AddDistance (DemoEntityManager* const scene, const dVector& origin)
  111. {
  112.     dVector size (1.0f, 1.0f, 1.0f);
  113.     NewtonBody* const box0 = CreateBox(scene, origin + dVector (0.0f,  5.0f + size.m_y + 0.25f, 0.0f, 0.0f), size.Scale (0.2f));
  114.     NewtonBody* const box1 = CreateCapule (scene, origin + dVector (0.0f,  5.0f, 0.0f, 0.0f), size);
  115.     NewtonBody* const box2 = CreateCapule (scene, origin + dVector (0.0f,  5.0 - size.m_y * 4.0f, 0.0f, 0.0f), size);
  116.  
  117.     dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
  118.     NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
  119.  
  120.     // connect first box to the world
  121.     dMatrix matrix0;
  122.     NewtonBodyGetMatrix (box1, &matrix0[0][0]);
  123.     pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
  124.     new CustomBallAndSocket (pinMatrix, box1, box0);
  125.  
  126.     // link the two boxes with a distance joint
  127.     dMatrix matrix1;
  128.     NewtonBodyGetMatrix (box2, &matrix1[0][0]);
  129.  
  130.     // get the origins
  131.     dVector pivot0 (matrix0.m_posit - dVector (0.0f, size.m_y, 0.0f, 0.0f));
  132.     dVector pivot1 (matrix1.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f));
  133.  
  134.     // connect bodies at a corner
  135.     new CustomPointToPoint (pivot1, pivot0, box2, box1);
  136.    
  137. #ifdef _USE_HARD_JOINTS
  138.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
  139.     NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
  140.     NewtonSkeletonContainerAttachBone(skeleton, box2, box1);
  141.     NewtonSkeletonContainerFinalize(skeleton);
  142. #endif
  143. }
  144.  
  145. static void AddLimitedBallAndSocket (DemoEntityManager* const scene, const dVector& origin)
  146. {
  147.     dVector size(1.0f, 1.0f, 1.0f);
  148.     NewtonBody* const base = CreateBox(scene, origin + dVector (0.0f,  5.0f + size.m_y + 0.25f, 0.0f, 0.0f), size.Scale (0.2f));
  149.     NewtonBody* const box0 = CreateCapule(scene, origin + dVector(0.0f, 5.0f, 0.0f, 0.0f), size);
  150.     NewtonBody* const box1 = CreateCapule(scene, origin + dVector(0.0f, 5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
  151.     NewtonBody* const box2 = CreateCapule(scene, origin + dVector(0.0f, 5.0 - size.m_y * 4.0f, 0.0f, 0.0f), size);
  152.  
  153.     NewtonBodySetMassMatrix(base, 0.0f, 0.0f, 0.0f, 0.0f);
  154.     dMatrix pinMatrix(dGrammSchmidt(dVector(0.0f, -1.0f, 0.0f, 0.0f)));
  155.  
  156.     // connect first box to the world
  157.     dMatrix matrix;
  158.     NewtonBodyGetMatrix(box0, &matrix[0][0]);
  159.     pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
  160.  
  161.     CustomLimitBallAndSocket* const joint0 = new CustomLimitBallAndSocket(pinMatrix, box0, base);
  162.     joint0->SetConeAngle (30.0f * 3.141592f / 180.0f);
  163.     joint0->SetTwistAngle (-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
  164.  
  165.     // connect first box1 to box0 the world
  166.     NewtonBodyGetMatrix(box1, &matrix[0][0]);
  167.     pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
  168.  
  169.     CustomLimitBallAndSocket* const joint1 = new CustomLimitBallAndSocket(pinMatrix, box1, box0);
  170.     joint1->SetConeAngle(30.0f * 3.141592f / 180.0f);
  171.     joint1->SetTwistAngle(-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
  172.  
  173.     // connect first box2 to box1 the world
  174.     NewtonBodyGetMatrix(box2, &matrix[0][0]);
  175.     pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
  176.  
  177.     CustomLimitBallAndSocket* const joint2 = new CustomLimitBallAndSocket(pinMatrix, box2, box1);
  178.     joint2->SetConeAngle(30.0f * 3.141592f / 180.0f);
  179.     joint2->SetTwistAngle(-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
  180.  
  181. #ifdef _USE_HARD_JOINTS
  182.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), base, NULL);
  183.     NewtonSkeletonContainerAttachBone(skeleton, box0, base);
  184.     NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
  185.     NewtonSkeletonContainerAttachBone(skeleton, box2, box1);
  186.     NewtonSkeletonContainerFinalize(skeleton);
  187. #endif
  188. }
  189.  
  190.  
  191. static void AddBallAndSockectWithFriction (DemoEntityManager* const scene, const dVector& origin)
  192. {
  193.     dVector size (1.0f, 1.0f, 1.0f);
  194.     NewtonBody* const base = CreateBox(scene, origin + dVector (0.0f,  5.0f + size.m_y + 0.25f, 0.0f, 0.0f), size.Scale (0.2f));
  195.     NewtonBody* const box0 = CreateCapule (scene, origin + dVector (0.0f,  5.0f, 0.0f, 0.0f), size);
  196.     NewtonBody* const box1 = CreateCapule (scene, origin + dVector (0.0f,  5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
  197.  
  198.     NewtonBodySetMassMatrix(base, 0.0f, 0.0f, 0.0f, 0.0f);
  199.     dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
  200.  
  201.     // connect first box to the world
  202.     dMatrix matrix0;
  203.     NewtonBodyGetMatrix (box0, &matrix0[0][0]);
  204.     pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
  205.     new CustomBallAndSocketWithFriction (pinMatrix, box0, base, 20.0f);
  206.  
  207.     // link the two boxes
  208.     dMatrix matrix1;
  209.     NewtonBodyGetMatrix (box1, & matrix1[0][0]);
  210.     pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
  211.     new CustomBallAndSocketWithFriction (pinMatrix, box1, box0, 10.0f);
  212.  
  213. #ifdef _USE_HARD_JOINTS
  214.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), base, NULL);
  215.     NewtonSkeletonContainerAttachBone(skeleton, box0, base);
  216.     NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
  217.     NewtonSkeletonContainerFinalize(skeleton);
  218. #endif
  219. }
  220.  
  221. static void Add6DOF (DemoEntityManager* const scene, const dVector& origin)
  222. {
  223.     dVector size (1.0f, 1.0f, 1.0f);
  224.     NewtonBody* const base = CreateBox(scene, origin + dVector (0.0f,  5.0f + size.m_y + 0.25f, 0.0f, 0.0f), size.Scale (0.2f));
  225.     NewtonBody* const box0 = CreateCapule (scene, origin + dVector (0.0f,  5.0f, 0.0f, 0.0f), size);
  226.     NewtonBody* const box1 = CreateCapule (scene, origin + dVector (0.0f,  5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
  227.  
  228.     const dFloat angle = 60.0f * 3.1415592f / 180.0f;
  229.     NewtonBodySetMassMatrix(base, 0.0f, 0.0f, 0.0f, 0.0f);
  230.     dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
  231.  
  232.     // connect first box to the world
  233.     dMatrix matrix0;
  234.     NewtonBodyGetMatrix (box0, & matrix0[0][0]);
  235.     pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
  236.     Custom6DOF* const joint0 = new Custom6DOF (pinMatrix, pinMatrix, box0, base);
  237.     joint0->SetAngularLimits (dVector (-angle, -angle, -angle, 0.0f), dVector (angle, angle, angle, 0.0f));
  238.  
  239.     // link the two boxes
  240.     dMatrix matrix1;
  241.     NewtonBodyGetMatrix (box1, &matrix1[0][0]);
  242.     pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
  243.     Custom6DOF* const joint1 = new Custom6DOF (pinMatrix, pinMatrix, box1, box0);
  244.     joint1->SetAngularLimits (dVector (-angle, -angle, -angle, 0.0f), dVector (angle, angle, angle, 0.0f));
  245.  
  246. #ifdef _USE_HARD_JOINTS
  247.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), base, NULL);
  248.     NewtonSkeletonContainerAttachBone(skeleton, box0, base);
  249.     NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
  250.     NewtonSkeletonContainerFinalize(skeleton);
  251. #endif
  252. }
  253.  
  254. static void AddUniversal(DemoEntityManager* const scene, const dVector& origin)
  255. {
  256.     dVector size(1.0f, 1.0f, 1.0f);
  257.     NewtonBody* const box0 = CreateBox(scene, origin + dVector(0.0f, 4.0f, 0.0f, 0.0f), dVector(0.25f, 0.25f, 4.0f, 0.0f));
  258.     NewtonBody* const box1 = CreateWheel(scene, origin + dVector(0.0f, 4.0f, 2.0f, 0.0f), 1.0f, 0.5f);
  259.     NewtonBody* const box2 = CreateWheel(scene, origin + dVector(0.0f, 4.0f, -2.0f, 0.0f), 1.0f, 0.5f);
  260.  
  261.     NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
  262.  
  263.     // align the object so that is looks nice
  264.     dMatrix matrix;
  265.     NewtonBodyGetMatrix(box1, &matrix[0][0]);
  266.     matrix = dYawMatrix (3.1416f * 0.5f) * matrix;
  267.     NewtonBodySetMatrix(box1, &matrix[0][0]);
  268.     ((DemoEntity*) NewtonBodyGetUserData(box1))->ResetMatrix (*scene, matrix);
  269.  
  270.     NewtonBodyGetMatrix(box2, &matrix[0][0]);
  271.     matrix = dYawMatrix(3.1416f * 0.5f) * matrix;
  272.     NewtonBodySetMatrix(box2, &matrix[0][0]);
  273.     ((DemoEntity*) NewtonBodyGetUserData(box2))->ResetMatrix (*scene, matrix);
  274.  
  275.  
  276.     // link the two boxes
  277.     NewtonBodyGetMatrix(box1, &matrix[0][0]);
  278.     CustomUniversal* const joint1 = new CustomUniversal(matrix, box1, box0);
  279.     joint1->EnableLimit_0(true);
  280.     joint1->SetLimis_0 (-5.0f * 3.141592f, 2.0f * 3.141592f);
  281.     joint1->EnableLimit_1(true);
  282.     joint1->SetLimis_1 (-3.0f * 3.141592f, 4.0f * 3.141592f);
  283.  
  284.     // link the two boxes
  285.     NewtonBodyGetMatrix(box2, &matrix[0][0]);
  286.     CustomUniversal* const joint2 = new CustomUniversal(matrix, box2, box0);
  287.     joint2->EnableLimit_0(true);
  288.     joint2->SetLimis_0 (-3.0f * 3.141592f, 5.0f * 3.141592f);
  289.     joint2->EnableLimit_1(true);
  290.     joint2->SetLimis_1(-4.0f * 3.141592f, 2.0f * 3.141592f);
  291.  
  292.  
  293. #ifdef _USE_HARD_JOINTS
  294.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
  295.     NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
  296.     NewtonSkeletonContainerAttachBone(skeleton, box2, box0);
  297.     NewtonSkeletonContainerFinalize(skeleton);
  298. #endif
  299. }
  300.  
  301.  
  302. class JoesRagdollJoint: public CustomBallAndSocket
  303. {
  304.     public:
  305.     dQuaternion m_target; // relative target rotation to reach at next timestep
  306.  
  307.     dFloat m_reduceError;
  308.     dFloat m_pin_length;
  309.     dFloat m_angularFriction;
  310.     dFloat m_stiffness;
  311.  
  312.     dFloat m_anim_speed;
  313.     dFloat m_anim_offset;
  314.     dFloat m_anim_time;
  315.    
  316.     JoesRagdollJoint(NewtonBody* child, NewtonBody* parent, const dMatrix &localMatrix0, const dMatrix &localMatrix1, NewtonWorld *world)
  317.         :CustomBallAndSocket(localMatrix0, localMatrix1, child, parent)
  318.     {
  319.         m_localMatrix0 = localMatrix0;
  320.         m_localMatrix1 = localMatrix1;
  321.  
  322.         m_target = dQuaternion(dVector(1.0f, 0, 0), 0.0f);
  323.         m_reduceError = 0.95f; // amount of error to reduce per timestep (more -> oszillation)
  324.         m_stiffness = 0.98f;
  325.         m_angularFriction = 300.0f;
  326.  
  327.         m_anim_speed = 0.0f;
  328.         m_anim_offset = 0.0f;
  329.         m_anim_time = 0.0f;
  330.     }
  331.  
  332.     dVector BodyGetPointVelocity(const NewtonBody* const body, const dVector &point)
  333.     {
  334.         dMatrix matrix;
  335.         dVector v(0.0f);
  336.         dVector w(0.0f);
  337.         dVector c(0.0f);
  338.         NewtonBodyGetVelocity(body, &v[0]);
  339.         NewtonBodyGetOmega(body, &w[0]);
  340.         NewtonBodyGetMatrix(body, &matrix[0][0]);
  341.         c = matrix.m_posit; // TODO: Does not handle COM offset !!!
  342.         return v + w.CrossProduct(point - c);
  343.     }
  344.  
  345.     void SubmitConstraints(dFloat timestep, int threadIndex)
  346.     {
  347.         dFloat invTimestep = 1.0f / timestep;
  348.  
  349.         dMatrix matrix0;
  350.         dMatrix matrix1;
  351.  
  352.         CalculateGlobalMatrix(matrix0, matrix1);
  353.  
  354.         if (m_anim_speed != 0.0f) // some animation to illustrate purpose
  355.         {
  356.             m_anim_time += timestep * m_anim_speed;
  357.             dFloat a0 = sin(m_anim_time);
  358.             dFloat a1 = m_anim_offset * 3.14f;
  359.             dVector axis(sin(a1), 0.0f, cos(a1));
  360.             //dVector axis (1,0,0);
  361.             m_target = dQuaternion(axis, a0 * 0.5f);
  362.         }
  363.  
  364.         // measure error
  365.         dQuaternion q0(matrix0);
  366.         dQuaternion q1(matrix1);
  367.         dQuaternion qt0 = m_target * q1;
  368.         dQuaternion qErr = ((q0.DotProduct(qt0) < 0.0f) ? dQuaternion(-q0.m_q0, q0.m_q1, q0.m_q2, q0.m_q3) : dQuaternion(q0.m_q0, -q0.m_q1, -q0.m_q2, -q0.m_q3)) * qt0;
  369.         qErr.Normalize();
  370.  
  371.         dFloat errorAngle = 2.0f * acos(dMax(dFloat(-1.0f), dMin(dFloat(1.0f), qErr.m_q0)));
  372.         dVector errorAngVel(0, 0, 0);
  373.  
  374.         dMatrix basis;
  375.         if (errorAngle > 1.0e-10f) {
  376.             dVector errorAxis(qErr.m_q1, qErr.m_q2, qErr.m_q3, 0.0f);
  377.             errorAxis = errorAxis.Scale(1.0f / dSqrt(errorAxis.DotProduct3(errorAxis)));
  378.             errorAngVel = errorAxis.Scale(errorAngle * invTimestep);
  379.  
  380.             basis = dGrammSchmidt(errorAxis);
  381.         } else {
  382.             basis = dMatrix(qt0, dVector(0.0f, 0.0f, 0.0f, 1.0f));
  383.         }
  384.  
  385.         dVector angVel0(0.0f);
  386.         dVector angVel1(0.0f);
  387.         NewtonBodyGetOmega(m_body0, (dFloat*)&angVel0);
  388.         NewtonBodyGetOmega(m_body1, (dFloat*)&angVel1);
  389.  
  390.         dVector angAcc = (errorAngVel.Scale(m_reduceError) - (angVel0 - angVel1)).Scale(invTimestep);
  391.  
  392.         CustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
  393.         // motors
  394.         for (int n = 0; n < 3; n++) {
  395.             // calculate the desired acceleration
  396.             dVector &axis = basis[n];
  397.             dFloat relAccel = angAcc.DotProduct3(axis);
  398.  
  399.             NewtonUserJointAddAngularRow(m_joint, 0.0f, &axis[0]);
  400.             NewtonUserJointSetRowAcceleration(m_joint, relAccel);
  401.             NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
  402.             NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
  403.             NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
  404.         }
  405.     }
  406. };
  407.  
  408. /*
  409. void AddJoesPoweredRagDoll (DemoEntityManager* const scene, const dVector& origin, const dFloat animSpeed, const int numSegments)
  410. {
  411.     dFloat height = 1.0f;
  412.     dFloat width = 4.0f;
  413.  
  414.     dVector size (width, height, width);
  415.     NewtonBody* parent = CreateBox (scene, origin + dVector (0.0f,  0.5f, 0.0f, 0.0f), size);
  416.  
  417. #ifdef _USE_HARD_JOINTS
  418.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate (scene->GetNewton(), parent, NULL);
  419. #endif
  420.  
  421.     for (int i=0; i < numSegments; i++)
  422.     {
  423.         dFloat height = 1.0f;
  424.         dFloat width = 0.5f;
  425.  
  426.         dVector size (width, height, width);
  427.         NewtonBody* child = CreateBox (scene, origin + dVector (0.0f,  0.5f + height * dFloat(i+1), 0.0f, 0.0f), size);
  428.  
  429.         dMatrix matrix0 = dGetIdentityMatrix(); matrix0.m_posit = dVector (0.0f, height*-0.5f, 0.0f, 1.0f);
  430.         dMatrix matrix1 = dGetIdentityMatrix(); matrix1.m_posit = dVector (0.0f, height*0.5f, 0.0f, 1.0f);
  431.         JoesRagdollJoint* joint = new JoesRagdollJoint (child, parent, matrix0, matrix1, scene->GetNewton());
  432.  
  433.         if (animSpeed != 0.0f) {
  434.             joint->m_anim_speed = animSpeed, joint->m_anim_offset = dFloat(i) / dFloat(numSegments); // animated      
  435.         }
  436.  
  437. #ifdef _USE_HARD_JOINTS
  438.         NewtonSkeletonContainerAttachBone (skeleton, child, parent);
  439. #endif
  440.         parent = child;
  441.     }
  442.  
  443. #ifdef _USE_HARD_JOINTS
  444.     NewtonSkeletonContainerFinalize(skeleton);
  445. #endif
  446. }*/
  447.  
  448. inline float randF (unsigned int time)
  449. {
  450.     time *= 1664525;
  451.     time ^= (time << 16);
  452.     time *= 16807;
  453.     return float(time) / float(0xFFFFFFFFu);
  454. }
  455.  
  456. void AddJoesPoweredRagDoll (DemoEntityManager* const scene, const dVector& origin, const dFloat animSpeed, const int numSegments,
  457.     const int numArms = 1,
  458.     const dFloat torsoHeight = 1.0f,
  459.     const dFloat torsoWidth = 4.0f,
  460.     const dFloat randomness = 0.0f,
  461.     const dFloat armHeight = 1.0f,
  462.     const dFloat armWidth = 0.5f,
  463.     const int pickMe = -1)
  464. {
  465.     dFloat height = torsoHeight;
  466.     dFloat width = torsoWidth;
  467.  
  468.     dVector size (width, height, width);
  469.     NewtonBody* torso = CreateBox (scene, origin + dVector (0.0f,  0.5f, 0.0f, 0.0f), size);
  470.     dMatrix torsoMatrix; NewtonBodyGetMatrix (torso, (dFloat*) &torsoMatrix);
  471.  
  472. #ifdef _USE_HARD_JOINTS
  473.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate (scene->GetNewton(), torso, NULL);
  474. #endif
  475.  
  476.     unsigned int bodyIndex = 0;
  477.     NewtonBody* pickBody = 0;
  478.     for (int j=0; j < numArms; j++)
  479.     {
  480.         dFloat angle = dFloat(j) / dFloat(numArms) * M_PI*2.0f;
  481.         dMatrix armRotation = dPitchMatrix(angle);
  482.         dMatrix armTransform = armRotation * torsoMatrix;
  483.        
  484.         NewtonBody* parent = torso;
  485.  
  486.         int numBodies = numSegments;
  487.         if (randomness > 0.0f) numBodies += int (randF(j) * dFloat(numSegments) + 0.5f);
  488.         for (int i=0; i < numBodies; i++)
  489.         {
  490.             dFloat height = armHeight;
  491.             dFloat width = armWidth;
  492.  
  493.             dVector size (width, height, width);
  494.             dVector pos (0.0f,  height * dFloat(i + (numArms>1 ? 2 : 1)), 0.0f, 0.0f);
  495.             NewtonBody* child = CreateBox (scene, pos, size);
  496.            
  497.             dMatrix bodyMatrix; NewtonBodyGetMatrix (child, (dFloat*) &bodyMatrix);
  498.             bodyMatrix = bodyMatrix * armTransform;
  499.             NewtonBodySetMatrix (child, (dFloat*) &bodyMatrix);
  500.  
  501.             dMatrix matrix0 = dGetIdentityMatrix(); matrix0.m_posit = dVector (0.0f, height*-0.5f, 0.0f, 1.0f);
  502.             dMatrix matrix1 = dGetIdentityMatrix(); matrix1.m_posit = dVector (0.0f, height*0.5f, 0.0f, 1.0f);
  503.             if (parent == torso)
  504.             {
  505.                 matrix1.m_posit.m_y += height;
  506.                 matrix1 = matrix1 * armRotation;
  507.             }
  508.             if (randomness > 0.0f)
  509.             {
  510.                 dMatrix rotation =  dPitchMatrix(randF(bodyIndex*3+0) * M_PI * 0.25f * randomness);
  511.                 rotation = rotation * dYawMatrix(randF(bodyIndex*3+1) * M_PI * 0.25f * randomness);
  512.                 rotation = rotation * dYawMatrix(randF(bodyIndex*3+2) * M_PI * 0.25f * randomness);
  513.                 matrix0 = matrix0 * rotation;
  514.             }
  515.             JoesRagdollJoint* joint = new JoesRagdollJoint (child, parent, matrix0, matrix1, scene->GetNewton());
  516.  
  517.             if (animSpeed != 0.0f) {
  518.                 joint->m_anim_speed = animSpeed, joint->m_anim_offset = dFloat(i) / dFloat(numBodies); // animated      
  519.             }
  520.  
  521.     #ifdef _USE_HARD_JOINTS
  522.             NewtonSkeletonContainerAttachBone (skeleton, child, parent);
  523.     #endif
  524.             parent = child;
  525.             if (bodyIndex == pickMe) pickBody = child;
  526.             bodyIndex++;
  527.         }
  528.     }
  529.  
  530. #ifdef _USE_HARD_JOINTS
  531.     NewtonSkeletonContainerFinalize(skeleton);
  532. #endif
  533.  
  534.     if (pickBody)
  535.     {
  536.         dMatrix matrix;
  537.         NewtonBodyGetMatrix(pickBody, &matrix[0][0]);
  538.        
  539.         CustomBallAndSocket* const joint = new CustomBallAndSocket(matrix, pickBody);
  540.        
  541. #ifdef _USE_HARD_JOINTS
  542.         NewtonSkeletonContainerAttachCyclingJoint (skeleton, joint->GetJoint());
  543. #endif
  544.     }
  545. }
  546.  
  547.  
  548.  
  549. static void AddPoweredRagDoll (DemoEntityManager* const scene, const dVector& origin)
  550. {
  551.     dVector size (1.0f, 1.0f, 1.0f);
  552.     NewtonBody* const box0 = CreateCapule(scene, origin + dVector(0.0f, 9.0f, 0.0f, 0.0f), size);
  553. //  NewtonBody* const box1 = CreateCapule(scene, origin + dVector(0.0f, 9.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
  554. //  NewtonBody* const box2 = CreateCapule(scene, origin + dVector(0.0f, 9.0 - size.m_y * 4.0f, 0.0f, 0.0f), size);
  555. //  NewtonBody* const box3 = CreateCapule(scene, origin + dVector(0.0f, 9.0 - size.m_y * 6.0f, 0.0f, 0.0f), size);
  556.  
  557.     dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
  558. //dMatrix pinMatrix (dGrammSchmidt (dVector (1.0f, 0.0f, 0.0f, 0.0f)));
  559.  
  560.     // connect first box to the world
  561.     dMatrix matrix0;
  562.     NewtonBodyGetMatrix (box0, & matrix0[0][0]);
  563.     pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
  564.     CustomControlledBallAndSocket* const joint0 = new CustomControlledBallAndSocket (pinMatrix, box0, NULL);
  565.     joint0->SetAngularVelocity (2000.0f * 3.141592f / 180.0f);
  566. //  joint0->SetPitchAngle (-45.0f * 3.141592f / 180.0f);
  567. //  joint0->SetYawAngle (-85.0f * 3.141592f / 180.0f);
  568. //  joint0->SetRollAngle (120.0f * 3.141592f / 180.0f);
  569.    
  570. joint0->SetPitchAngle (90.0f * 3.141592f / 180.0f);
  571. /*
  572.     // link the two boxes
  573.     dMatrix matrix1;
  574.     NewtonBodyGetMatrix (box1, &matrix1[0][0]);
  575.     pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
  576.     CustomControlledBallAndSocket* const joint1 = new CustomControlledBallAndSocket (pinMatrix, box0, box1);
  577.     joint1->SetAngularVelocity (1000.0f * 3.141592f / 180.0f);
  578.     joint1->SetPitchAngle (45.0f * 3.141592f / 180.0f);
  579.     joint1->SetYawAngle ( 30.0f * 3.141592f / 180.0f);
  580.     joint1->SetRollAngle (25.0f * 3.141592f / 180.0f);
  581.  
  582.     // link next box
  583.     dMatrix matrix2;
  584.     NewtonBodyGetMatrix(box2, &matrix2[0][0]);
  585.     pinMatrix.m_posit = (matrix1.m_posit + matrix2.m_posit).Scale(0.5f);
  586.     CustomControlledBallAndSocket* const joint2 = new CustomControlledBallAndSocket(pinMatrix, box1, box2);
  587.     joint2->SetAngularVelocity(1000.0f * 3.141592f / 180.0f);
  588.     joint2->SetPitchAngle(45.0f * 3.141592f / 180.0f);
  589.     joint2->SetYawAngle(30.0f * 3.141592f / 180.0f);
  590.     joint2->SetRollAngle(25.0f * 3.141592f / 180.0f);
  591.  
  592.     // link next box
  593.     dMatrix matrix3;
  594.     NewtonBodyGetMatrix(box3, &matrix3[0][0]);
  595.     pinMatrix.m_posit = (matrix2.m_posit + matrix3.m_posit).Scale(0.5f);
  596.     CustomControlledBallAndSocket* const joint3 = new CustomControlledBallAndSocket(pinMatrix, box2, box3);
  597.     joint3->SetAngularVelocity(1000.0f * 3.141592f / 180.0f);
  598.     joint3->SetPitchAngle(45.0f * 3.141592f / 180.0f);
  599.     joint3->SetYawAngle(30.0f * 3.141592f / 180.0f);
  600.     joint3->SetRollAngle(25.0f * 3.141592f / 180.0f);
  601.  
  602. #ifdef _USE_HARD_JOINTS
  603.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
  604.     NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
  605.     NewtonSkeletonContainerAttachBone(skeleton, box2, box1);
  606.     NewtonSkeletonContainerAttachBone(skeleton, box3, box2);
  607.     NewtonSkeletonContainerFinalize(skeleton);
  608. #endif
  609. */
  610. }
  611.  
  612. void AddHinge (DemoEntityManager* const scene, const dVector& origin)
  613. {
  614.     dVector size (1.5f, 1.5f, 0.125f);
  615.     NewtonBody* parent = CreateBox(scene, origin + dVector (-0.8f, 4.0f, 0.0f, 0.0f), dVector (0.2f, 0.125f, 0.125f));
  616.     NewtonBodySetMassMatrix(parent, 0.0f, 0.0f, 0.0f, 0.0f);
  617.     //the joint pin is the first row of the matrix, to make a upright pin we
  618.     //take the x axis and rotate by 90 degree around the y axis
  619.     dMatrix localPin (dRollMatrix(90.0f * 3.141592f / 180.0f));
  620.  
  621.     dMatrix matrix;
  622.     dVector position (origin);
  623.     position.m_y += 4.0f;
  624.     NewtonBody* child = NULL;
  625.  
  626. #ifdef _USE_HARD_JOINTS
  627.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), parent, NULL);
  628. #endif
  629.  
  630.     int count = 4;
  631.     for (int i = 0; i < count; i ++) {
  632.         child = CreateBox (scene, position, size);
  633.         NewtonBodyGetMatrix(child, &matrix[0][0]);
  634.         matrix.m_posit += dVector(-size.m_x * 0.5f, 0.0f, 0.0f);
  635.         matrix = localPin * matrix;
  636.         CustomHinge* const hinge = new CustomHinge (matrix, child, parent);
  637.  
  638. #ifdef _USE_HARD_JOINTS
  639.         NewtonSkeletonContainerAttachBone (skeleton, child, parent);   
  640. #endif
  641.  
  642.         hinge->EnableLimits (true);
  643.         hinge->SetLimits (-45.0f * 3.141592f / 180.0f, 45.0f * 3.141592f / 180.0f);
  644.         hinge->SetFriction(20.0f);
  645.         parent = child;
  646.         position.m_x += size.m_x;
  647.     }
  648.  
  649. /*
  650.     // link the two boxes
  651.     NewtonBody* const heavyBox = CreateBox (scene, position, dVector (1.5f, 1.5f, 1.5f));
  652.     NewtonBodyGetMatrix(heavyBox, &matrix[0][0]);
  653.     NewtonBodySetMassProperties(heavyBox, 10.0f, NewtonBodyGetCollision(heavyBox));
  654.     matrix.m_posit += dVector(-size.m_x * 0.5f, 0.0f, 0.0f);
  655.     matrix = localPin * matrix;
  656.     CustomHinge* const hinge = new CustomHinge(matrix, heavyBox, parent);
  657.     hinge->EnableLimits(true);
  658.     hinge->SetLimits(-45.0f * 3.141592f / 180.0f, 45.0f * 3.141592f / 180.0f);
  659.     hinge->SetFriction(20.0f);
  660. */
  661. #ifdef _USE_HARD_JOINTS
  662. //  NewtonSkeletonContainerAttachBone (skeleton, heavyBox, parent);
  663.     NewtonSkeletonContainerFinalize (skeleton);
  664. #endif
  665.  
  666. }
  667.  
  668. static void AddSlider (DemoEntityManager* const scene, const dVector& origin)
  669. {
  670.     // make a reel static
  671.     NewtonBody* const box0 = CreateBox (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), dVector (8.0f, 0.25f, 0.25f, 0.0f));
  672.     NewtonBody* const box1 = CreateWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), 1.0f, 0.5f);
  673.  
  674.     dMatrix matrix;
  675.     NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
  676.  
  677.     // connect the bodies by a Slider joint
  678.     NewtonBodyGetMatrix (box1, &matrix[0][0]);
  679.     CustomSlider* const slider = new CustomSlider (matrix, box1, box0);
  680.  
  681.     // enable limit of first axis
  682.     slider->EnableLimits(true);
  683.  
  684.     // set limit on second axis
  685.     slider->SetLimits (-4.0f, 4.0f);
  686.  
  687. #ifdef _USE_HARD_JOINTS
  688.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
  689.     NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
  690.     NewtonSkeletonContainerFinalize(skeleton);
  691. #endif
  692. }
  693.  
  694. static void AddSlidingContact(DemoEntityManager* const scene, const dVector& origin)
  695. {
  696.     // make a reel static
  697.     NewtonBody* const box0 = CreateBox(scene, origin + dVector(0.0f, 4.0f, 0.0f, 0.0f), dVector(8.0f, 0.25f, 0.25f, 0.0f));
  698.     NewtonBody* const box1 = CreateWheel(scene, origin + dVector(0.0f, 4.0f, 0.0f, 0.0f), 1.0f, 0.5f);
  699.  
  700.     dMatrix matrix;
  701.  
  702.     NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
  703.  
  704.     // connect the bodies by a Slider joint
  705.     NewtonBodyGetMatrix(box1, &matrix[0][0]);
  706.     CustomSlidingContact* const slider = new CustomSlidingContact(matrix, box1, box0);
  707.     slider->EnableLinearLimits (true);
  708.     slider->SetLinearLimis (-4.0f, 4.0f);
  709.  
  710.     // enable limit of first axis
  711.     slider->EnableAngularLimits(true);
  712.     slider->SetAngularLimis (-7.0f * 3.1416f, 5.0f * 3.1416f);
  713.  
  714. #ifdef _USE_HARD_JOINTS
  715.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
  716.     NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
  717.     NewtonSkeletonContainerFinalize(skeleton);
  718. #endif
  719. }
  720.  
  721.  
  722. static void AddCylindrical (DemoEntityManager* const scene, const dVector& origin)
  723. {
  724.     // make a reel static
  725.     NewtonBody* const box0 = CreateCylinder (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), 0.25f, 8.0f);
  726.     NewtonBody* const box1 = CreateWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), 1.0f, 0.5f);
  727.  
  728.     dMatrix matrix;
  729.     NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
  730.  
  731.     // connect the bodies by a CorkScrew joint
  732.     NewtonBodyGetMatrix (box1, &matrix[0][0]);
  733.     CustomCorkScrew* const cylinder = new CustomCorkScrew (matrix, box1, box0);
  734.  
  735.     // enable limit of first axis
  736.     cylinder->EnableLinearLimits(true);
  737.     cylinder->SetLinearLimis (-4.0f, 4.0f);
  738.  
  739.     // set angular limit on second axis
  740.     cylinder->EnableAngularLimits(true);
  741.     cylinder->SetAngularLimis(-4.0f * 3.1416f, 6.0f * 3.1416f);
  742.  
  743. #ifdef _USE_HARD_JOINTS
  744.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
  745.     NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
  746.     NewtonSkeletonContainerFinalize(skeleton);
  747. #endif
  748. }
  749.  
  750.  
  751. static CustomHinge* AddHingeWheel (DemoEntityManager* const scene, const dVector& origin, dFloat radius, dFloat height, NewtonBody* const parent)
  752. {
  753.     NewtonBody* const wheel = CreateWheel (scene, origin, height, radius);
  754.  
  755.     // the joint pin is the first row of the matrix
  756.     //dMatrix localPin (dRollMatrix(90.0f * 3.141592f / 180.0f));
  757.     dMatrix localPin (dGetIdentityMatrix());
  758.     dMatrix matrix;
  759.     NewtonBodyGetMatrix (wheel, & matrix[0][0]);
  760.     matrix = localPin * matrix;
  761.  
  762.     // connect first box to the world
  763.     return new CustomHinge (matrix, wheel, parent);
  764. }
  765.  
  766.  
  767. static void AddGear (DemoEntityManager* const scene, const dVector& origin)
  768. {
  769.     NewtonBody* const box0 = CreateCylinder(scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.25f, 4.0f);
  770.  
  771.     // this is a fix joint
  772.     NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
  773.  
  774.     // connect two bodies with a hinge
  775.     CustomHinge* const hinge0 = AddHingeWheel (scene, origin + dVector (-1.0f, 4.0f, 0.0f), 0.5f, 1.0f, box0);
  776.     CustomHinge* const hinge1 = AddHingeWheel (scene, origin + dVector ( 1.0f, 4.0f, 0.0f), 0.5f, 1.0f, box0);
  777.  
  778.     NewtonBody* const body0 = hinge0->GetBody0();
  779.     NewtonBody* const body1 = hinge1->GetBody0();
  780.  
  781.     dMatrix matrix0;
  782.     dMatrix matrix1;
  783.     NewtonBodyGetMatrix (body0, &matrix0[0][0]);
  784.     NewtonBodyGetMatrix (body1, &matrix1[0][0]);
  785.  
  786.     // relate the two body motion with a gear joint
  787.     dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
  788.     dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
  789.     new CustomGear (4.0f, pin0, pin1, body0, body1);
  790.  
  791. #ifdef _USE_HARD_JOINTS
  792.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
  793. //  NewtonSkeletonContainerAttachBone(skeleton, box0, NULL);
  794.     NewtonSkeletonContainerAttachBone(skeleton, hinge0->GetBody0(), box0);
  795.     NewtonSkeletonContainerAttachBone(skeleton, hinge1->GetBody0(), box0);
  796.     NewtonSkeletonContainerFinalize(skeleton);
  797. #endif
  798.  
  799. }
  800.  
  801.  
  802. static CustomSlider* AddSliderWheel (DemoEntityManager* const scene, const dVector& origin, dFloat radius, dFloat height, NewtonBody* const parent)
  803. {
  804.     NewtonBody* const wheel = CreateWheel (scene, origin, height, radius);
  805.  
  806.     // the joint pin is the first row of the matrix
  807.     //dMatrix localPin (dRollMatrix(90.0f * 3.141592f / 180.0f));
  808.     dMatrix localPin (dGetIdentityMatrix());
  809.     dMatrix matrix;
  810.     NewtonBodyGetMatrix (wheel, & matrix[0][0]);
  811.     matrix = localPin * matrix;
  812.  
  813.     // connect first box to the world
  814.     return new CustomSlider (matrix, wheel, parent);
  815. }
  816.  
  817. void AddPulley (DemoEntityManager* const scene, const dVector& origin)
  818. {
  819.     NewtonBody* const reel0 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f));
  820.     // this is just for show
  821.     NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 0.0f), dVector(4.0f, 0.25f, 0.25f));
  822.     NewtonBodySetMassMatrix (reel0, 0.0f, 0.0f, 0.0f, 0.0f);
  823.     NewtonBodySetMassMatrix (reel1, 0.0f, 0.0f, 0.0f, 0.0f);
  824.    
  825.     dMatrix matrix;
  826.     CustomSlider* const slider0 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel0);
  827.     CustomSlider* const slider1 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);
  828.  
  829.     slider0->EnableLimits(true);
  830.     slider0->SetLimits (-2.0f, 2.0f);
  831.  
  832.     NewtonBody* const body0 = slider0->GetBody0();
  833.     NewtonBody* const body1 = slider1->GetBody0();
  834.  
  835.     dMatrix matrix0;
  836.     dMatrix matrix1;
  837.     NewtonBodyGetMatrix (body0, &matrix0[0][0]);
  838.     NewtonBodyGetMatrix (body1, &matrix1[0][0]);
  839.  
  840.     dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
  841.     dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
  842.     new CustomPulley (4.0f, pin0, pin1, body0, body1);
  843.  
  844. #ifdef _USE_HARD_JOINTS
  845.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), reel0, NULL);
  846.     NewtonSkeletonContainerAttachBone(skeleton, slider0->GetBody0(), reel0);
  847.     NewtonSkeletonContainerAttachBone(skeleton, slider1->GetBody0(), reel0);
  848.     NewtonSkeletonContainerFinalize(skeleton);
  849. #endif
  850.  
  851.     // make an aggregate for disabling collisions
  852.     void* const aggregate = NewtonCollisionAggregateCreate (scene->GetNewton());
  853.     NewtonCollisionAggregateSetSelfCollision (aggregate, 0);
  854.     NewtonCollisionAggregateAddBody (aggregate, reel0);
  855.     NewtonCollisionAggregateAddBody (aggregate, reel1);
  856.     NewtonCollisionAggregateAddBody (aggregate, body0);
  857.     NewtonCollisionAggregateAddBody (aggregate, body1);
  858. }
  859.  
  860.  
  861. static CustomCorkScrew* AddCylindricalWheel (DemoEntityManager* const scene, const dVector& origin, dFloat radius, dFloat height, NewtonBody* const parent)
  862. {
  863.     NewtonBody* const wheel = CreateWheel (scene, origin, height, radius);
  864.  
  865.     // the joint pin is the first row of the matrix
  866.     dMatrix matrix;
  867.     NewtonBodyGetMatrix (wheel, &matrix[0][0]);
  868.  
  869.     return new CustomCorkScrew (matrix, wheel, parent);
  870. }
  871.  
  872.  
  873. static void AddGearAndRack (DemoEntityManager* const scene, const dVector& origin)
  874. {
  875.     NewtonBody* const reel0 = CreateCylinder(scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.25f, 4.0f);
  876.     NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f));
  877.    
  878.     dMatrix matrix;
  879.     NewtonBodySetMassMatrix(reel0, 0.0f, 0.0f, 0.0f, 0.0f);
  880.     NewtonBodySetMassMatrix(reel1, 0.0f, 0.0f, 0.0f, 0.0f);
  881.  
  882.     CustomHinge* const hinge0 = AddHingeWheel (scene, origin + dVector (-1.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);
  883.     CustomHinge* const hinge1 = AddHingeWheel (scene, origin + dVector ( 1.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);
  884.     CustomCorkScrew* const cylinder = AddCylindricalWheel(scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel0);
  885.  
  886.     cylinder->EnableLinearLimits(true);
  887.     cylinder->SetLinearLimis(-2.0f, 2.0f);
  888.  
  889.     NewtonBody* const body0 = hinge0->GetBody0();
  890.     NewtonBody* const body1 = hinge1->GetBody0();
  891.     NewtonBody* const body2 = cylinder->GetBody0();
  892.  
  893.     dMatrix matrix0;
  894.     dMatrix matrix1;
  895.     dMatrix matrix2;
  896.  
  897.     NewtonBodyGetMatrix (body0, &matrix0[0][0]);
  898.     NewtonBodyGetMatrix (body1, &matrix1[0][0]);
  899.     NewtonBodyGetMatrix (body2, &matrix2[0][0]);
  900.  
  901.     dVector pin0 (matrix0.RotateVector(dVector( 1.0f, 0.0f, 0.0f)));
  902.     dVector pin1 (matrix1.RotateVector(dVector( 1.0f, 0.0f, 0.0f)));
  903.     dVector pin2 (matrix2.RotateVector(dVector( 1.0f, 0.0f, 0.0f)));
  904.  
  905.     new CustomGear (5.0f, pin0, pin2, body0, body2);
  906.     new CustomRackAndPinion (0.125f, pin1, pin2, body1, body2);
  907.  
  908. #ifdef _USE_HARD_JOINTS
  909.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), reel0, NULL);
  910.     NewtonSkeletonContainerAttachBone(skeleton, hinge0->GetBody0(), reel0);
  911.     NewtonSkeletonContainerAttachBone(skeleton, hinge1->GetBody0(), reel0);
  912.     NewtonSkeletonContainerAttachBone(skeleton, cylinder->GetBody0(), reel0);
  913.     NewtonSkeletonContainerFinalize(skeleton);
  914. #endif
  915.  
  916.     // make an aggregate for disabling collisions
  917.     void* const aggregate = NewtonCollisionAggregateCreate(scene->GetNewton());
  918.     NewtonCollisionAggregateSetSelfCollision(aggregate, 0);
  919.     NewtonCollisionAggregateAddBody(aggregate, reel0);
  920.     NewtonCollisionAggregateAddBody(aggregate, reel1);
  921.     NewtonCollisionAggregateAddBody(aggregate, body0);
  922.     NewtonCollisionAggregateAddBody(aggregate, body1);
  923.     NewtonCollisionAggregateAddBody(aggregate, body2);
  924. }
  925.  
  926.  
  927. class MyPathFollow: public CustomPathFollow
  928. {
  929.     public:
  930.     MyPathFollow(const dMatrix& pinAndPivotFrame, NewtonBody* const body, NewtonBody* const pathBody)
  931.         :CustomPathFollow (pinAndPivotFrame, body, pathBody)
  932.     {
  933.     }
  934.  
  935.     void GetPointAndTangentAtLocation (const dVector& location,  dVector& positOut, dVector& tangentOut) const
  936.     {
  937.         DemoEntity* const pathEntity = (DemoEntity*) NewtonBodyGetUserData (GetBody1());
  938.         DemoBezierCurve* const mesh = (DemoBezierCurve*)pathEntity->GetMesh();
  939.         const dBezierSpline& spline = mesh->m_curve;
  940.  
  941.         dMatrix matrix;
  942.         NewtonBodyGetMatrix(GetBody1(), &matrix[0][0]);
  943.  
  944.         dVector p(matrix.UntransformVector(location));
  945.         dBigVector point;
  946.         dFloat64 knot = spline.FindClosestKnot (point, p, 4);
  947.         dBigVector tangent (spline.CurveDerivative (knot));
  948.         tangent = tangent.Scale (1.0 / sqrt (tangent.DotProduct3(tangent)));
  949.  
  950.         positOut = matrix.TransformVector (dVector (point.m_x, point.m_y, point.m_z));
  951.         tangentOut = dVector (tangent.m_x, tangent.m_y, tangent.m_z);
  952.     }
  953. };
  954.  
  955.  
  956. static void AddPathFollow (DemoEntityManager* const scene, const dVector& origin)
  957. {
  958.     // create a Bezier Spline path for AI car to drive
  959.     NewtonBody* const pathBody = CreateBox(scene, origin, dVector(4.0f, 0.25f, 0.25f));
  960.     NewtonBodySetMassMatrix(pathBody, 0.0f, 0.0f, 0.0f, 0.0f);
  961.     DemoEntity* const rollerCosterPath = (DemoEntity*) NewtonBodyGetUserData(pathBody);
  962.    
  963.     dBezierSpline spline;
  964.     dFloat64 knots[] = {0.0f, 1.0f / 5.0f, 2.0f / 5.0f, 3.0f / 5.0f, 4.0f / 5.0f, 1.0f};
  965.  
  966.     dBigVector control[] =
  967.     {
  968.         dBigVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 1.0f),
  969.         dBigVector(150.0f - 100.0f, 10.0f, 150.0f - 250.0f, 1.0f),
  970.         dBigVector(175.0f - 100.0f, 30.0f, 250.0f - 250.0f, 1.0f),
  971.         dBigVector(200.0f - 100.0f, 70.0f, 250.0f - 250.0f, 1.0f),
  972.         dBigVector(215.0f - 100.0f, 20.0f, 250.0f - 250.0f, 1.0f),
  973.         dBigVector(150.0f - 100.0f, 50.0f, 350.0f - 250.0f, 1.0f),
  974.         dBigVector( 50.0f - 100.0f, 30.0f, 250.0f - 250.0f, 1.0f),
  975.         dBigVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 1.0f),
  976.     };
  977.  
  978.     spline.CreateFromKnotVectorAndControlPoints(3, sizeof (knots) / sizeof (knots[0]), knots, control);
  979.  
  980.     DemoBezierCurve* const mesh = new DemoBezierCurve (spline);
  981.     rollerCosterPath->SetMesh(mesh, dGetIdentityMatrix());
  982.    
  983.     mesh->SetVisible(true);
  984.     mesh->SetRenderResolution(500);
  985.     mesh->Release();
  986.    
  987.     const int count = 32;
  988.     NewtonBody* bodies[count];
  989.  
  990.     dBigVector point0;
  991.    
  992.     dVector positions[count + 1];
  993.     dFloat64 knot = spline.FindClosestKnot(point0, dBigVector (dVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 0.0f)), 4);
  994.     positions[0] = dVector (point0.m_x, point0.m_y, point0.m_z, 0.0);
  995.     for (int i = 0; i < count; i ++) {
  996.         dBigVector point1;
  997.         dBigVector tangent(spline.CurveDerivative(knot));
  998.         tangent = tangent.Scale (1.0 / sqrt (tangent.DotProduct3(tangent)));
  999.         knot = spline.FindClosestKnot(point1, dBigVector (point0 + tangent.Scale (2.0f)), 4);
  1000.         point0 = point1;
  1001.         positions[i + 1] = dVector (point1.m_x, point1.m_y, point1.m_z, 0.0);
  1002.     }
  1003.  
  1004.     dMatrix pathBodyMatrix;
  1005.     NewtonBodyGetMatrix(pathBody, &pathBodyMatrix[0][0]);
  1006.  
  1007.     dFloat attachmentOffset = 0.8f;
  1008.     for (int i = 0; i < count; i ++) {
  1009.         dMatrix matrix;
  1010.         bodies[i] = CreateWheel(scene, dVector (0.0f, 0.0f, 0.0f, 0.0f), 1.0f, 0.5f);
  1011.         NewtonBodySetLinearDamping(bodies[i], 0.0f);
  1012.         NewtonBody* const box = bodies[i];
  1013.         NewtonBodyGetMatrix(box, &matrix[0][0]);
  1014.  
  1015.         dVector location0(positions[i + 0].m_x, positions[i + 0].m_y, positions[i + 0].m_z, 0.0);
  1016.         dVector location1(positions[i + 1].m_x, positions[i + 1].m_y, positions[i + 1].m_z, 0.0);
  1017.  
  1018.         location0 = pathBodyMatrix.TransformVector(location0);
  1019.         location1 = pathBodyMatrix.TransformVector(location1);
  1020.  
  1021.         dVector dir (location1 - location0);
  1022.         dir.m_w = 0.0f;
  1023.         matrix.m_front = dir.Scale (1.0f / dSqrt (dir.DotProduct3(dir)));
  1024.         matrix.m_right = matrix.m_front.CrossProduct(matrix.m_up);
  1025.         matrix.m_right = matrix.m_right.Scale(1.0f / dSqrt(matrix.m_right.DotProduct3(matrix.m_right)));
  1026.         matrix.m_up = matrix.m_right.CrossProduct(matrix.m_front);
  1027.         matrix.m_posit = pathBodyMatrix.TransformVector(dVector (positions[i].m_x, positions[i].m_y - attachmentOffset, positions[i].m_z, 1.0));
  1028.         dMatrix matrix1 (dYawMatrix(0.5f * 3.141692f) * matrix);
  1029.  
  1030.         NewtonBodySetMatrix(box, &matrix1[0][0]);
  1031.         DemoEntity* const ent = (DemoEntity*)NewtonBodyGetUserData(box);
  1032.         ent->ResetMatrix(*scene, matrix1);
  1033.  
  1034.         matrix.m_posit = pathBodyMatrix.TransformVector(dVector(positions[i].m_x, positions[i].m_y, positions[i].m_z, 1.0));
  1035.         new MyPathFollow(matrix, box, pathBody);
  1036.  
  1037.         dVector veloc (dir.Scale (20.0f));
  1038.         NewtonBodySetVelocity(box, &veloc[0]);
  1039.     }
  1040.    
  1041.     for (int i = 1; i < count; i ++) {
  1042.         NewtonBody* const box0 = bodies[i - 1];
  1043.         NewtonBody* const box1 = bodies[i];
  1044.  
  1045.         dMatrix matrix0;
  1046.         dMatrix matrix1;
  1047.         NewtonBodyGetMatrix(box0, &matrix0[0][0]);
  1048.         NewtonBodyGetMatrix(box1, &matrix1[0][0]);
  1049.  
  1050.         matrix0.m_posit.m_y += attachmentOffset;
  1051.         matrix1.m_posit.m_y += attachmentOffset;
  1052.  
  1053.         //new CustomDistanceRope (matrix1.m_posit, matrix0.m_posit, box1, box0);
  1054.         new CustomPointToPoint (matrix1.m_posit, matrix0.m_posit, box1, box0);
  1055.     }
  1056.  
  1057.     void* const aggregate = NewtonCollisionAggregateCreate (scene->GetNewton());
  1058.     for (int i = 0; i < count; i ++) {
  1059.         NewtonCollisionAggregateAddBody(aggregate, bodies[i]);
  1060.     }
  1061.     NewtonCollisionAggregateSetSelfCollision (aggregate, false);
  1062.  
  1063. #ifdef _USE_HARD_JOINTS
  1064.     NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), bodies[0], NULL);
  1065.     for (int i = 1; i < count; i++) {
  1066.         NewtonSkeletonContainerAttachBone(skeleton, bodies[i], bodies[i - 1]);
  1067.     }
  1068.     NewtonSkeletonContainerFinalize(skeleton);
  1069. #endif
  1070. }
  1071.  
  1072. void StandardJoints (DemoEntityManager* const scene)
  1073. {
  1074.     scene->CreateSkyBox();
  1075.  
  1076.     // customize the scene after loading
  1077.     // set a user friction variable in the body for variable friction demos
  1078.     // later this will be done using LUA script
  1079.     dMatrix offsetMatrix (dGetIdentityMatrix());
  1080.  
  1081.     CreateLevelMesh (scene, "flatPlane.ngd", 1);
  1082.     //CreateLevelMesh (scene, "flatPlane1.ngd", 1);
  1083.  
  1084.     dVector location (0.0f);
  1085.     dVector size (1.5f, 2.0f, 2.0f, 0.0f);
  1086.  
  1087.     AddJoesPoweredRagDoll(scene, dVector(0.0f, 0.0f, -25.0f), 0.0f, 20);
  1088.     AddJoesPoweredRagDoll(scene, dVector(0.0f, 0.0f, 5.0f), 1.5f, 4);
  1089.     AddJoesPoweredRagDoll(scene, dVector(0.0f, 0.0f, 15.0f), 0.0f, 4);
  1090.  
  1091.     AddJoesPoweredRagDoll(scene, dVector( 5.0f, 20.0f, 0.0f), 0.0f, 4, 4, 1.0f, 1.0f);
  1092.     AddJoesPoweredRagDoll(scene, dVector(40.0f, 20.0f, 0.0f), 0.0f, 7, 2, 0.4f, 0.4f, 1.3f);
  1093.     AddJoesPoweredRagDoll(scene, dVector(80.0f, 20.0f, 0.0f), 0.0f, 5, 3, 0.4f, 0.4f, 1.0f, 0.5f, 0.5f);
  1094.     AddJoesPoweredRagDoll(scene, dVector( 5.0f, 20.0f, 0.0f), 0.0f, 3, 5, 1.0f, 1.0f, 1.3f, 0.5f, 0.5f, 4); // no picking problem here
  1095.  
  1096.     AddDistance (scene, dVector (-20.0f, 0.0f, -25.0f));
  1097.     AddLimitedBallAndSocket (scene, dVector (-20.0f, 0.0f, -20.0f));
  1098. //  AddPoweredRagDoll (scene, dVector (-20.0f, 0.0f, -15.0f));
  1099.     AddBallAndSockectWithFriction (scene, dVector (-20.0f, 0.0f, -10.0f));
  1100.     Add6DOF (scene, dVector (-20.0f, 0.0f, -5.0f));
  1101.  
  1102.     AddHinge (scene, dVector (-20.0f, 0.0f, 0.0f));
  1103.     AddSlider (scene, dVector (-20.0f, 0.0f, 5.0f));
  1104.     AddCylindrical (scene, dVector (-20.0f, 0.0f, 10.0f));
  1105.     AddUniversal (scene, dVector (-20.0f, 0.0f, 15.0f));
  1106.     AddGear (scene, dVector (-20.0f, 0.0f, 20.0f));
  1107.     AddPulley (scene, dVector (-20.0f, 0.0f, 25.0f));
  1108.     AddGearAndRack (scene, dVector (-20.0f, 0.0f, 30.0f));
  1109.     AddSlidingContact (scene, dVector (-20.0f, 0.0f, 35.0f));
  1110.     AddPathFollow (scene, dVector (20.0f, 0.0f, 0.0f));
  1111.  
  1112.     // place camera into position
  1113.     dMatrix camMatrix (dGetIdentityMatrix());
  1114.     dQuaternion rot (camMatrix);
  1115.     dVector origin (-50.0f, 5.0f, 0.0f, 0.0f);
  1116.     scene->SetCameraMatrix(rot, origin);
  1117. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!