static void AddGear (DemoEntityManager* const scene, const dVector& origin) { NewtonBody* const box0 = CreateCylinder(scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.25f, 4.0f); // this is a fix joint NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f); // connect two bodies with a hinge CustomHinge* const hinge0 = AddHingeWheel (scene, origin + dVector (-1.0f, 4.0f, 0.0f), 0.5f, 1.0f, box0); CustomHinge* const hinge1 = AddHingeWheel (scene, origin + dVector ( 1.0f, 4.0f, 0.0f), 0.5f, 1.0f, box0); NewtonBody* const body0 = hinge0->GetBody0(); NewtonBody* const body1 = hinge1->GetBody0(); dMatrix matrix0; dMatrix matrix1; NewtonBodyGetMatrix (body0, &matrix0[0][0]); NewtonBodyGetMatrix (body1, &matrix1[0][0]); // relate the two body motion with a gear joint dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f))); dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f))); new CustomGear (4.0f, pin0, pin1, body0, body1); #ifdef _USE_HARD_JOINTS NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL); // NewtonSkeletonContainerAttachBone(skeleton, box0, NULL); NewtonSkeletonContainerAttachBone(skeleton, hinge0->GetBody0(), box0); NewtonSkeletonContainerAttachBone(skeleton, hinge1->GetBody0(), box0); NewtonSkeletonContainerFinalize(skeleton); #endif }
static void AddGearAndRack (DemoEntityManager* const scene, const dVector& origin) { NewtonBody* const reel0 = CreateCylinder(scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.25f, 4.0f); NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f)); dMatrix matrix; //connect the box0 to the base by a fix joint (a hinge with zero limit) NewtonBodyGetMatrix(reel0, &matrix[0][0]); CustomHinge* const fixJoint0 = new CustomHinge(matrix, reel0, NULL); fixJoint0->EnableLimits(true); fixJoint0->SetLimis(0.0f, 0.0f); NewtonBodyGetMatrix(reel1, &matrix[0][0]); CustomHinge* const fixJoint1 = new CustomHinge(matrix, reel1, NULL); fixJoint1->EnableLimits(true); fixJoint1->SetLimis(0.0f, 0.0f); CustomHinge* const hinge0 = AddHingeWheel (scene, origin + dVector (-1.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0); CustomHinge* const hinge1 = AddHingeWheel (scene, origin + dVector ( 1.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0); CustomCorkScrew* const cylinder = AddCylindricalWheel(scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel1); cylinder->EnableLinearLimits(true); cylinder->SetLinearLimis(-2.0f, 2.0f); NewtonBody* const body0 = hinge0->GetBody0(); NewtonBody* const body1 = hinge1->GetBody0(); NewtonBody* const body2 = cylinder->GetBody0(); dMatrix matrix0; dMatrix matrix1; dMatrix matrix2; NewtonBodyGetMatrix (body0, &matrix0[0][0]); NewtonBodyGetMatrix (body1, &matrix1[0][0]); NewtonBodyGetMatrix (body2, &matrix2[0][0]); dVector pin0 (matrix0.RotateVector(dVector( 1.0f, 0.0f, 0.0f))); dVector pin1 (matrix1.RotateVector(dVector( 1.0f, 0.0f, 0.0f))); dVector pin2 (matrix2.RotateVector(dVector( 1.0f, 0.0f, 0.0f))); new CustomGear (1.0f, pin0, pin2, body0, body2); new CustomRackAndPinion (1.0f, pin1, pin2, body1, body2); #ifdef _USE_HARD_JOINTS NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), NULL, NULL); NewtonSkeletonContainerAttachBone(skeleton, reel0, NULL); NewtonSkeletonContainerAttachBone(skeleton, hinge0->GetBody0(), reel0); NewtonSkeletonContainerAttachBone(skeleton, hinge1->GetBody0(), reel0); NewtonSkeletonContainerAttachBone(skeleton, reel1, NULL); NewtonSkeletonContainerAttachBone(skeleton, cylinder->GetBody0(), reel1); NewtonSkeletonContainerFinalize(skeleton); #endif }
static void AddGearAndRack (DemoEntityManager* const scene, const dVector& origin) { NewtonBody* const reel0 = CreateCylinder(scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.25f, 4.0f); NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f)); dMatrix matrix; NewtonBodySetMassMatrix(reel0, 0.0f, 0.0f, 0.0f, 0.0f); NewtonBodySetMassMatrix(reel1, 0.0f, 0.0f, 0.0f, 0.0f); CustomHinge* const hinge0 = AddHingeWheel (scene, origin + dVector (-1.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0); CustomHinge* const hinge1 = AddHingeWheel (scene, origin + dVector ( 1.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0); CustomCorkScrew* const cylinder = AddCylindricalWheel(scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel0); cylinder->EnableLinearLimits(true); cylinder->SetLinearLimis(-2.0f, 2.0f); NewtonBody* const body0 = hinge0->GetBody0(); NewtonBody* const body1 = hinge1->GetBody0(); NewtonBody* const body2 = cylinder->GetBody0(); dMatrix matrix0; dMatrix matrix1; dMatrix matrix2; NewtonBodyGetMatrix (body0, &matrix0[0][0]); NewtonBodyGetMatrix (body1, &matrix1[0][0]); NewtonBodyGetMatrix (body2, &matrix2[0][0]); dVector pin0 (matrix0.RotateVector(dVector( 1.0f, 0.0f, 0.0f))); dVector pin1 (matrix1.RotateVector(dVector( 1.0f, 0.0f, 0.0f))); dVector pin2 (matrix2.RotateVector(dVector( 1.0f, 0.0f, 0.0f))); new CustomGear (1.0f, pin0, pin2, body0, body2); new CustomRackAndPinion (1.0f, pin1, pin2, body1, body2); #ifdef _USE_HARD_JOINTS NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), reel0, NULL); NewtonSkeletonContainerAttachBone(skeleton, hinge0->GetBody0(), reel0); NewtonSkeletonContainerAttachBone(skeleton, hinge1->GetBody0(), reel0); NewtonSkeletonContainerAttachBone(skeleton, cylinder->GetBody0(), reel0); NewtonSkeletonContainerFinalize(skeleton); #endif // make an aggregate for disabling collisions void* const aggregate = NewtonCollisionAggregateCreate(scene->GetNewton()); NewtonCollisionAggregateSetSelfCollision(aggregate, 0); NewtonCollisionAggregateAddBody(aggregate, reel0); NewtonCollisionAggregateAddBody(aggregate, reel1); NewtonCollisionAggregateAddBody(aggregate, body0); NewtonCollisionAggregateAddBody(aggregate, body1); NewtonCollisionAggregateAddBody(aggregate, body2); }
TEST(ButtonProtocolTest, CheckRuns) { ButtonLed blue(1, LOW); ButtonLed gold(2, LOW); ButtonLed pin1(10, LOW); ButtonLed pin2(11, LOW); ButtonLed pin3(12, LOW); ButtonLed* buttons[] = {&pin1,&pin1,&pin2,&pin2,&pin3,&pin3}; ButtonProtocol bp(buttons, (uint8_t)6, &blue, &gold); bp.check(); }
void AddPulley (DemoEntityManager* const scene, const dVector& origin) { NewtonBody* const reel0 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f)); NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 0.0f), dVector(4.0f, 0.25f, 0.25f)); // NewtonBodySetMassMatrix (reel0, 0.0f, 0.0f, 0.0f, 0.0f); // NewtonBodySetMassMatrix (reel1, 0.0f, 0.0f, 0.0f, 0.0f); dMatrix matrix; //connect the box0 to the base by a fix joint (a hinge with zero limit) NewtonBodyGetMatrix(reel0, &matrix[0][0]); CustomHinge* const fixJoint0 = new CustomHinge(matrix, reel0, NULL); fixJoint0->EnableLimits(true); fixJoint0->SetLimis(0.0f, 0.0f); NewtonBodyGetMatrix(reel1, &matrix[0][0]); CustomHinge* const fixJoint1 = new CustomHinge(matrix, reel1, NULL); fixJoint1->EnableLimits(true); fixJoint1->SetLimis(0.0f, 0.0f); CustomSlider* const slider0 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel0); CustomSlider* const slider1 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel1); slider0->EnableLimits(true); slider0->SetLimis (-2.0f, 2.0f); NewtonBody* const body0 = slider0->GetBody0(); NewtonBody* const body1 = slider1->GetBody0(); dMatrix matrix0; dMatrix matrix1; NewtonBodyGetMatrix (body0, &matrix0[0][0]); NewtonBodyGetMatrix (body1, &matrix1[0][0]); dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f))); dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f))); new CustomPulley (4.0f, pin0, pin1, body0, body1); #ifdef _USE_HARD_JOINTS NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), NULL, NULL); NewtonSkeletonContainerAttachBone(skeleton, reel0, NULL); NewtonSkeletonContainerAttachBone(skeleton, slider0->GetBody0(), reel0); NewtonSkeletonContainerAttachBone(skeleton, reel1, NULL); NewtonSkeletonContainerAttachBone(skeleton, slider1->GetBody0(), reel1); NewtonSkeletonContainerFinalize(skeleton); #endif }
TEST(ButtonProtocolTest, HandleOtherFrame) { OpenLcbCanBuffer b; ButtonLed blue(1, LOW); ButtonLed gold(2, LOW); ButtonLed pin1(10, LOW); ButtonLed pin2(11, LOW); ButtonLed pin3(12, LOW); ButtonLed* buttons[] = {&pin1,&pin1,&pin2,&pin2,&pin3,&pin3}; ButtonProtocol bp(buttons, (uint8_t)6, &blue, &gold); EXPECT_FALSE(bp.receivedFrame(&b)); }
TEST(ButtonProtocolTest, HandleButtonProtocolRequestFrame) { OpenLcbCanBuffer b; b.setOpenLcbMTI(0x948); ButtonLed blue(1, LOW); ButtonLed gold(2, LOW); ButtonLed pin1(10, LOW); ButtonLed pin2(11, LOW); ButtonLed pin3(12, LOW); ButtonLed* buttons[] = {&pin1,&pin1,&pin2,&pin2,&pin3,&pin3}; ButtonProtocol bp(buttons, (uint8_t)6, &blue, &gold); EXPECT_TRUE(bp.receivedFrame(&b)); }
TEST(ButtonProtocolTest, Ctor) { // This test is named "Ctor", and belongs to the "ButtonProtocolTest" // test case. //OpenLcbCanBuffer b; ButtonLed blue(1, LOW); ButtonLed gold(2, LOW); ButtonLed pin1(10, LOW); ButtonLed pin2(11, LOW); ButtonLed pin3(12, LOW); ButtonLed* buttons[] = {&pin1,&pin1,&pin2,&pin2,&pin3,&pin3}; ButtonProtocol bp(buttons, (uint8_t)6, &blue, &gold); }
void AddPulley (DemoEntityManager* const scene, const dVector& origin) { NewtonBody* const reel0 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f)); // this is just for show NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 0.0f), dVector(4.0f, 0.25f, 0.25f)); NewtonBodySetMassMatrix (reel0, 0.0f, 0.0f, 0.0f, 0.0f); NewtonBodySetMassMatrix (reel1, 0.0f, 0.0f, 0.0f, 0.0f); dMatrix matrix; CustomSlider* const slider0 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel0); CustomSlider* const slider1 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0); slider0->EnableLimits(true); slider0->SetLimits (-2.0f, 2.0f); NewtonBody* const body0 = slider0->GetBody0(); NewtonBody* const body1 = slider1->GetBody0(); dMatrix matrix0; dMatrix matrix1; NewtonBodyGetMatrix (body0, &matrix0[0][0]); NewtonBodyGetMatrix (body1, &matrix1[0][0]); dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f))); dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f))); new CustomPulley (4.0f, pin0, pin1, body0, body1); #ifdef _USE_HARD_JOINTS NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), reel0, NULL); NewtonSkeletonContainerAttachBone(skeleton, slider0->GetBody0(), reel0); NewtonSkeletonContainerAttachBone(skeleton, slider1->GetBody0(), reel0); NewtonSkeletonContainerFinalize(skeleton); #endif // make an aggregate for disabling collisions void* const aggregate = NewtonCollisionAggregateCreate (scene->GetNewton()); NewtonCollisionAggregateSetSelfCollision (aggregate, 0); NewtonCollisionAggregateAddBody (aggregate, reel0); NewtonCollisionAggregateAddBody (aggregate, reel1); NewtonCollisionAggregateAddBody (aggregate, body0); NewtonCollisionAggregateAddBody (aggregate, body1); }
void AddRollingBeats (NewtonWorld* nWorld) { dFloat mass; dFloat Ixx; dFloat Iyy; dFloat Izz; NewtonBody* bar; NewtonCollision* collision; dMatrix location (GetIdentityMatrix()); location.m_posit.m_x = 5.0f; location.m_posit.m_y = 2.0f; location.m_posit.m_z = -2.0f; dVector size (10.0f, 0.25f, 0.25f); bar = NULL; // ///////////////////////////////////////////////////////////////////////////////////// // // create a bar and attach it to the world with a hinge with limits // // //////////////////////////////////////////////////////////////////////////////////// { CustomHinge* joint; RenderPrimitive* visualObject; // create the a graphic character (use a visualObject as our body visualObject = new CylinderPrimitive (location, size.m_y, size.m_x); // create a collision primitive to be shared by all links collision = NewtonCreateCylinder (nWorld, size.m_y, size.m_x, NULL); // craete the bar body bar = NewtonCreateBody(nWorld, collision); NewtonReleaseCollision (nWorld, collision); // attach graphic object to the rigid body NewtonBodySetUserData(bar, visualObject); // set a destructor function NewtonBodySetDestructorCallback (bar, PhysicsBodyDestructor); // set the tranform call back function NewtonBodySetTransformCallback (bar, PhysicsSetTransform); // set the force and torque call back funtion NewtonBodySetForceAndTorqueCallback (bar,PhysicsApplyGravityForce); // calculate a acurate momenet of inertia mass = 5.0f; Ixx = 0.7f * mass * (size.m_y * size.m_y + size.m_z * size.m_z) / 12.0f; Iyy = 0.7f * mass * (size.m_x * size.m_x + size.m_z * size.m_z) / 12.0f; Izz = 0.7f * mass * (size.m_x * size.m_x + size.m_y * size.m_y) / 12.0f; // set the mass matrix NewtonBodySetMassMatrix (bar, mass, Ixx, Iyy, Izz); // set the matrix for both the rigid nody and the graphic body NewtonBodySetMatrix (bar, &location[0][0]); PhysicsSetTransform (bar, &location[0][0]); dVector pin (0.0f, 1.0f, 0.0f); dVector pivot (location.m_posit); pivot.m_x -= size.m_x * 0.5f; // connect these two bodies by a ball and sockect joint //joint = NewtonConstraintCreateHinge (nWorld, &pivot.m_x, &pin.m_x, link0, link1); joint = new CustomHinge (pivot, pin, bar, NULL); // no limits //joint->EnableLimits (true); //joint->SetAngleLimis (-30.0f * 3.1416f/180.0f, 30.0f * 3.1416f/180.0f); } { // //////////////////////////////////////////////////////////////////////////////////// // // add a sliding visualObject with limits // NewtonBody* beat; CustomSlider* joint; RenderPrimitive* visualObject; dMatrix beatLocation (location); dVector beatSize (0.5f, 2.0f, 2.0f); beatLocation.m_posit.m_x += size.m_x * 0.25f; // create the a graphic character (use a visualObject as our body visualObject = new BoxPrimitive (beatLocation, beatSize); // create a collision primitive to be shared by all links collision = NewtonCreateBox (nWorld, beatSize.m_x, beatSize.m_y, beatSize.m_z, NULL); beat = NewtonCreateBody(nWorld, collision); NewtonReleaseCollision (nWorld, collision); // attach graphic object to the rigid body NewtonBodySetUserData(beat, visualObject); // set a destyuctor function NewtonBodySetDestructorCallback (beat, PhysicsBodyDestructor); // set the tranform call back function NewtonBodySetTransformCallback (beat, PhysicsSetTransform); // set the force and torque call back funtion NewtonBodySetForceAndTorqueCallback (beat,PhysicsApplyGravityForce); // calculate a acurate momenet of inertia mass = 5.0f; Ixx = 0.7f * mass * (beatSize.m_y * beatSize.m_y + beatSize.m_z * beatSize.m_z) / 12.0f; Iyy = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_z * beatSize.m_z) / 12.0f; Izz = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_y * beatSize.m_y) / 12.0f; // set the mass matrix NewtonBodySetMassMatrix (beat, mass, Ixx, Iyy, Izz); // set the matrix for both the rigid nody and the graphic body NewtonBodySetMatrix (beat, &beatLocation[0][0]); PhysicsSetTransform (beat, &beatLocation[0][0]); // set the pivot relative for the first bar dVector pivot (beatLocation.m_posit); dVector pin (beatLocation.m_front); joint = new CustomSlider (pivot, pin, beat, bar); // claculate the minimum and maximum limit for this joints dFloat minLimits = ((location.m_posit.m_x - beatLocation.m_posit.m_x) - size.m_x * 0.5f); dFloat maxLimits = ((location.m_posit.m_x - beatLocation.m_posit.m_x) + size.m_x * 0.5f); joint->EnableLimits(true); joint->SetLimis (minLimits, maxLimits); } { // //////////////////////////////////////////////////////////////////////////////////// // // add a corkscrew visualObject with limits // // //////////////////////////////////////////////////////////////////////////////////// NewtonBody* beat; CustomCorkScrew* joint; RenderPrimitive* visualObject; dMatrix beatLocation (location); dVector beatSize (0.5f, 1.25f, 1.25f); beatLocation.m_posit.m_x -= size.m_x * 0.25f; // create the a graphic character (use a visualObject as our body //visualObject = new BoxPrimitive (beatLocation, beatSize); visualObject = new ChamferCylinderPrimitive (beatLocation, beatSize.m_y, beatSize.m_x); // create a collision primitive to be shared by all links collision = NewtonCreateChamferCylinder (nWorld, beatSize.m_y, beatSize.m_x, NULL); beat = NewtonCreateBody(nWorld, collision); NewtonReleaseCollision (nWorld, collision); // attach graphic object to the rigid body NewtonBodySetUserData(beat, visualObject); // set a destyuctor function NewtonBodySetDestructorCallback (beat, PhysicsBodyDestructor); // set the tranform call back function NewtonBodySetTransformCallback (beat, PhysicsSetTransform); // set the force and torque call back funtion NewtonBodySetForceAndTorqueCallback (beat,PhysicsApplyGravityForce); // calculate a acurate momenet of inertia mass = 5.0f; Ixx = 0.7f * mass * (beatSize.m_y * beatSize.m_y + beatSize.m_z * beatSize.m_z) / 12.0f; Iyy = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_z * beatSize.m_z) / 12.0f; Izz = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_y * beatSize.m_y) / 12.0f; // set the mass matrix NewtonBodySetMassMatrix (beat, mass, Ixx, Iyy, Izz); // set the matrix for both the rigid nody and the graphic body NewtonBodySetMatrix (beat, &beatLocation[0][0]); PhysicsSetTransform (beat, &beatLocation[0][0]); // set the pivot relative for the first bar dVector pivot (beatLocation.m_posit); dVector pin (beatLocation.m_front); joint = new CustomCorkScrew (pivot, pin, beat, bar); // claculate the minimum and maximum limit for this joints dFloat minLimits = ((location.m_posit.m_x - beatLocation.m_posit.m_x) - size.m_x * 0.5f); dFloat maxLimits = ((location.m_posit.m_x - beatLocation.m_posit.m_x) + size.m_x * 0.5f); joint->EnableLimits(true); joint->SetLimis (minLimits, maxLimits); } { // //////////////////////////////////////////////////////////////////////////////////// // // add a universal joint visualObject with limits // // //////////////////////////////////////////////////////////////////////////////////// NewtonBody* beat; CustomUniversal* joint; RenderPrimitive* visualObject; dMatrix beatLocation (location); dVector beatSize (0.5f, 1.25f, 1.25f); beatLocation.m_posit.m_x -= size.m_x * 0.5f; // create the a graphic character (use a visualObject as our body //visualObject = new BoxPrimitive (beatLocation, beatSize); visualObject = new ChamferCylinderPrimitive (beatLocation, beatSize.m_y, beatSize.m_x); // create a collision primitive to be shared by all links collision = NewtonCreateChamferCylinder (nWorld, beatSize.m_y, beatSize.m_x, NULL); beat = NewtonCreateBody(nWorld, collision); NewtonReleaseCollision (nWorld, collision); // attach graphic object to the rigid body NewtonBodySetUserData(beat, visualObject); // set a destyuctor function NewtonBodySetDestructorCallback (beat, PhysicsBodyDestructor); // set the tranform call back function NewtonBodySetTransformCallback (beat, PhysicsSetTransform); // set the force and torque call back funtion NewtonBodySetForceAndTorqueCallback (beat,PhysicsApplyGravityForce); // calculate a acurate momenet of inertia mass = 5.0f; Ixx = 0.7f * mass * (beatSize.m_y * beatSize.m_y + beatSize.m_z * beatSize.m_z) / 12.0f; Iyy = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_z * beatSize.m_z) / 12.0f; Izz = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_y * beatSize.m_y) / 12.0f; // set the mass matrix NewtonBodySetMassMatrix (beat, mass, Ixx, Iyy, Izz); // set the matrix for both the rigid nody and the graphic body NewtonBodySetMatrix (beat, &beatLocation[0][0]); PhysicsSetTransform (beat, &beatLocation[0][0]); // set the pivot relative for the first bar dVector pivot (beatLocation.m_posit); dVector pin0 (beatLocation.m_front); dVector pin1 (beatLocation.m_up); // tell this joint to destroiy its local private data when destroyed joint = new CustomUniversal (pivot, pin0, pin1, beat, bar); } { // //////////////////////////////////////////////////////////////////////////////////// // // add a universal joint visualObject with limits // // //////////////////////////////////////////////////////////////////////////////////// NewtonBody* beat; CustomUniversal* joint; RenderPrimitive* visualObject; dMatrix beatLocation (location); dVector beatSize (0.5f, 1.25f, 1.25f); beatLocation.m_posit.m_x = size.m_x; // create the a graphic character (use a visualObject as our body //visualObject = new BoxPrimitive (beatLocation, beatSize); visualObject = new ChamferCylinderPrimitive (beatLocation, beatSize.m_y, beatSize.m_x); // create a collision primitive to be shared by all links collision = NewtonCreateChamferCylinder (nWorld, beatSize.m_y, beatSize.m_x, NULL); beat = NewtonCreateBody(nWorld, collision); NewtonReleaseCollision (nWorld, collision); // attach graphic object to the rigid body NewtonBodySetUserData(beat, visualObject); // set a destyuctor function NewtonBodySetDestructorCallback (beat, PhysicsBodyDestructor); // set the tranform call back function NewtonBodySetTransformCallback (beat, PhysicsSetTransform); // set the force and torque call back funtion NewtonBodySetForceAndTorqueCallback (beat,PhysicsApplyGravityForce); // calculate a acurate momenet of inertia mass = 5.0f; Ixx = 0.7f * mass * (beatSize.m_y * beatSize.m_y + beatSize.m_z * beatSize.m_z) / 12.0f; Iyy = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_z * beatSize.m_z) / 12.0f; Izz = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_y * beatSize.m_y) / 12.0f; // set the mass matrix NewtonBodySetMassMatrix (beat, mass, Ixx, Iyy, Izz); // set the matrix for both the rigid nody and the graphic body NewtonBodySetMatrix (beat, &beatLocation[0][0]); PhysicsSetTransform (beat, &beatLocation[0][0]); // set the pivot relative for the first bar dVector pivot (beatLocation.m_posit); dVector pin0 (beatLocation.m_front.Scale(-1.0f)); dVector pin1 (beatLocation.m_up); // tell this joint to destroiy its local private data when destroyed joint = new CustomUniversal (pivot, pin0, pin1, beat, bar); } }
int main (int argc, char *argv[]) { signal(SIGINT, INThandler); /* Init */ #if TERSENOV ==1 /*Tersenov*/ Point<2> pout1 ( 0., 3.); Point<2> pout2 (12., 0.); Point<2> pin1 ( 1., 2.); Point<2> pin2 (11., 1.); #else /*Mu*/ Point<2> pout1 (-20., 10.); Point<2> pout2 ( 20.,-10.); Point<2> pin1 (-10., 2.); Point<2> pin2 ( 10.,-2.); #endif SineGordon_in *old_solver_in = NULL; Laplace_out *old_solver_out = NULL; std::vector< Functions::FEFieldFunction<2> * > FEFieldFunc_in_vec; std::vector< Functions::FEFieldFunction<2> * > FEFieldFunc_out_vec; InitialGuessFunction IGFunc(FEFieldFunc_in_vec, FEFieldFunc_out_vec, pin1, pin2); if (argc != 3) {std::cout<<"Wrong input!"<<std::endl; exit(1);} IGFunc.GAMMA = atof(argv[1]); std::cout<<"GAMMA : "<<IGFunc.GAMMA<<std::endl; IGFunc.DELTA = atof(argv[2]); std::cout<<"DELTA : "<<IGFunc.DELTA<<std::endl; // IGFunc.EPSILON = atof(argv[3]); std::cout<<"EPSILON : "<<IGFunc.EPSILON<<std::endl; // IGFunc.ZETA = atof(argv[4]); std::cout<<"ZETA : "<<IGFunc.ZETA<<std::endl; set_new_bound_vals(IGFunc, pin1, pin2); // #if NOF_FIRST_ITERS_TO_PRINT != 0 // SineGordon_in *solver_in_dummy = new SineGordon_in(IGFunc, pin1, pin2); // Laplace_out *solver_out_dummy = new Laplace_out(IGFunc, pin1, pin2, pout1, pout2); // print_output(IGFunc, solver_in_dummy, solver_out_dummy, "--initial", 0, false); // #endif for (unsigned int k=1; ; k++) { printf("### main() -- Step 1 (init) (iter: %d)\n", k); assert(FEFieldFunc_in_vec.size() == k-1); assert(FEFieldFunc_out_vec.size() == k-1); /* Step 2: solve inside */ printf("### main() -- Step 2 (SineGordon in)\n"); SineGordon_in *solver_in = new SineGordon_in(IGFunc, pin1, pin2); FEFieldFunc_in_vec.push_back(solver_in->run(old_solver_in)); old_solver_in = solver_in; /* Step 3: solve outside */ printf("### main() -- Step 3 (Laplace out)\n"); Laplace_out *solver_out = new Laplace_out(IGFunc, pin1, pin2, pout1, pout2); FEFieldFunc_out_vec.push_back(solver_out->run(old_solver_out)); old_solver_out = solver_out; IGFunc.current_depth++; //this corresponds to the nof times that the two problems have been solved /* Step 5: check for convergence */ printf("### main() -- Step 5 (check conv.)\n"); cpy_bound_vals(); set_new_bound_vals(IGFunc, pin1, pin2); if (k == MAX_ITERS) {going_out = true;} if (k<=NOF_FIRST_ITERS_TO_PRINT) {print_output(IGFunc, solver_in, solver_out, "", k, false);} #if PRINT_FIRST_ITERS__THEN_STOP == 1 else {break;} #endif #if STOP_ON_NORM_RAISE == 1 if (chk_conv(k)) {going_out = true;} #else if (chk_conv(k)) {break;} #endif if (going_out) { print_output(IGFunc, solver_in, solver_out, "--last", k, true); break; } } }