Added a construction that looks like #170 to test stability of the fixed joint

Samples/Tests/Constraints/FixedConstraintTest.cpp

@@ -71,19 +71,90 @@ void FixedConstraintTest::Initialize()
 		}
 	}
 
-	// Two light bodies attached to a heavy body (gives issues if the wrong anchor point is chosen)
-	Body *light1 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), Vec3(-5.0f, 7.0f, -5.2f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
-	mBodyInterface->AddBody(light1->GetID(), EActivation::Activate);
-	Body *heavy = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(5.0f)), Vec3(-5.0f, 7.0f, 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
-	mBodyInterface->AddBody(heavy->GetID(), EActivation::Activate);
-	Body *light2 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), Vec3(-5.0f, 7.0f, 5.2f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
-	mBodyInterface->AddBody(light2->GetID(), EActivation::Activate);
-
-	FixedConstraintSettings light1_heavy;
-	light1_heavy.SetPoint(*light1, *heavy);
-	mPhysicsSystem->AddConstraint(light1_heavy.Create(*light1, *heavy));
-
-	FixedConstraintSettings heavy_light2;
-	heavy_light2.SetPoint(*heavy, *light2);
-	mPhysicsSystem->AddConstraint(heavy_light2.Create(*heavy, *light2));
+	{
+		// Two light bodies attached to a heavy body (gives issues if the wrong anchor point is chosen)
+		Body *light1 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), Vec3(-5.0f, 7.0f, -5.2f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		mBodyInterface->AddBody(light1->GetID(), EActivation::Activate);
+		Body *heavy = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(5.0f)), Vec3(-5.0f, 7.0f, 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		mBodyInterface->AddBody(heavy->GetID(), EActivation::Activate);
+		Body *light2 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), Vec3(-5.0f, 7.0f, 5.2f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		mBodyInterface->AddBody(light2->GetID(), EActivation::Activate);
+
+		FixedConstraintSettings light1_heavy;
+		light1_heavy.SetPoint(*light1, *heavy);
+		mPhysicsSystem->AddConstraint(light1_heavy.Create(*light1, *heavy));
+
+		FixedConstraintSettings heavy_light2;
+		heavy_light2.SetPoint(*heavy, *light2);
+		mPhysicsSystem->AddConstraint(heavy_light2.Create(*heavy, *light2));
+	}
+
+	{
+		// A tower of beams and crossbeams (note that it is not recommended to make constructs with this many fixed constraints, this is not always stable)
+		Vec3 base_position(0, 25, -40.0f);
+		Quat base_rotation = Quat::sRotation(Vec3::sAxisZ(), -0.5f * JPH_PI);
+
+		Ref<BoxShape> pillar = new BoxShape(Vec3(0.1f, 1.0f, 0.1f), 0.0f);
+		Ref<BoxShape> beam = new BoxShape(Vec3(0.01f, 1.5f, 0.1f), 0.0f);
+
+		Body *prev_pillars[4] = { &Body::sFixedToWorld, &Body::sFixedToWorld, &Body::sFixedToWorld, &Body::sFixedToWorld };
+
+		Vec3 center(5.0f, 0, 0);
+		for (int y = 0; y < 10; ++y)
+		{
+			// Create pillars
+			Body *pillars[4];
+			for (int i = 0; i < 4; ++i)
+			{
+				Quat rotation = Quat::sRotation(Vec3::sAxisY(), i * 0.5f * JPH_PI);
+
+				pillars[i] = mBodyInterface->CreateBody(BodyCreationSettings(pillar, base_position + base_rotation * (center + rotation * Vec3(1.0f, 1.0f, 1.0f)), base_rotation, EMotionType::Dynamic, Layers::MOVING));
+				pillars[i]->SetCollisionGroup(CollisionGroup(group_filter, 0, 0)); // For convenience, we disable collisions between all objects in the tower
+				mBodyInterface->AddBody(pillars[i]->GetID(), EActivation::Activate);
+			}
+
+			for (int i = 0; i < 4; ++i)
+			{
+				Quat rotation = Quat::sRotation(Vec3::sAxisY(), i * 0.5f * JPH_PI);
+
+				// Create cross beam
+				Body *cross = mBodyInterface->CreateBody(BodyCreationSettings(beam, base_position + base_rotation * (center + rotation * Vec3(1.105f, 1.0f, 0.0f)), base_rotation * rotation * Quat::sRotation(Vec3::sAxisX(), 0.3f * JPH_PI), EMotionType::Dynamic, Layers::MOVING));
+				cross->SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+				mBodyInterface->AddBody(cross->GetID(), EActivation::Activate);
+
+				// Attach cross beam to pillars
+				for (int j = 0; j < 2; ++j)
+				{
+					FixedConstraintSettings constraint;
+					constraint.SetPoint(*cross, *pillars[i]);
+					mPhysicsSystem->AddConstraint(constraint.Create(*cross, *pillars[(i + j) % 4]));
+				}
+
+				// Attach to previous pillar
+				if (prev_pillars[i] != nullptr)
+				{
+					FixedConstraintSettings constraint;
+					constraint.SetPoint(*prev_pillars[i], *pillars[i]);
+					mPhysicsSystem->AddConstraint(constraint.Create(*prev_pillars[i], *pillars[i]));
+				}
+
+				prev_pillars[i] = pillars[i];
+			}
+
+			center += Vec3(0.0f, 2.0f, 0.0f);
+		}
+
+		// Create top
+		Body *top = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(1.2f, 0.1f, 1.2f)), base_position + base_rotation * (center + Vec3(0.0f, 0.1f, 0.0f)), base_rotation, EMotionType::Dynamic, Layers::MOVING));
+		top->SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+		mBodyInterface->AddBody(top->GetID(), EActivation::Activate);
+
+		// Attach top to pillars
+		for (int i = 0; i < 4; ++i)
+		{
+			FixedConstraintSettings constraint;
+			constraint.SetPoint(*top, *prev_pillars[i]);
+			mPhysicsSystem->AddConstraint(constraint.Create(*top, *prev_pillars[i]));
+		}
+	}
 }