Beispiel #1
0
    //void createStack(const PxTransform& t, PxU32 size, PxReal halfExtent){
    void createStack(PxTransform t, uint size, float halfExtent)
    {
        //PxShape* shape = gPhysics->createShape(PxBoxGeometry(halfExtent, halfExtent, halfExtent), *gMaterial);
        PxShapePtr shape = gPhysics.createShape(new PxBoxGeometry(halfExtent, halfExtent, halfExtent), gMaterial);

        //for(PxU32 i=0; i<size;i++){
        // for(PxU32 j=0;j<size-i;j++){
        for (int i = 0; i < size; i++)
        {
            for (int j = 0; j < size - i; j++)
            {
                //PxTransform localTm(PxVec3(PxReal(j*2) - PxReal(size-i), PxReal(i*2+1), 0) * halfExtent);
                //PxRigidDynamic* body = gPhysics->createRigidDynamic(t.transform(localTm));
                //body->attachShape(*shape);
                //PxRigidBodyExt::updateMassAndInertia(*body, 10.0f);
                //gScene->addActor(*body);
                PxTransform       localTm = new PxTransform(new PxVec3(((j * 2) - (size - i)) * halfExtent, (i * 2 + 1) * halfExtent, 0));
                PxRigidDynamicPtr body    = gPhysics.createRigidDynamic(t.transform(localTm));
                body.attachShape(shape);
                PxRigidBodyExt.updateMassAndInertia(body, 10);
                gScene.addActor(body);
            }
        }
        //shape->release();
        shape.release();
    }
    /**
     * Creates two example collections:
     * - collection with actors and joints that can be instantiated multiple times in the scene
     * - collection with shared objects
     */
    static void createCollections(ref PxCollection *sharedCollection, ref PxCollection *actorCollection, ref PxSerializationRegistry sr)
    {
        PxMaterial *material = gPhysics->createMaterial(0.5f, 0.5f, 0.6f);

        float         halfLength = 2.0f, height = 25.0f;
        PxVec3        offset    = new(halfLength, 0, 0);
        PxRigidActor *prevActor = (PxRigidActor *)PxCreateStatic(ref *gPhysics, new PxTransform(new PxVec3(0, height, 0)), new PxSphereGeometry(halfLength), ref *material, new PxTransform(offset)); //BIOQUIRK: Base cast

        PxShape *shape = gPhysics->createShape(new PxBoxGeometry(halfLength, 1.0f, 1.0f), *material,
                                               false, PxShapeFlags.eVISUALIZATION | PxShapeFlags.eSCENE_QUERY_SHAPE | PxShapeFlags.eSIMULATION_SHAPE); //BIOQUIRK: Missing defaults

        for (uint i = 1; i < 8; i++)
        {
            PxTransform     tm      = new(new PxVec3((float)(i * 2) * halfLength, height, 0));
            PxRigidDynamic *dynamic = gPhysics->createRigidDynamic(tm);
            dynamic->attachShape(ref *shape);
            PxRigidBodyExt.updateMassAndInertia(ref *dynamic, 10.0f);

            PxSphericalJointCreate(ref *gPhysics, prevActor, new PxTransform(offset), dynamic, new PxTransform(offset.operator_Minus())); //BIOQUIRK: Operator overload
            prevActor = (PxRigidActor *)dynamic;                                                                                          //BIOQUIRK: Base cast
        }

        sharedCollection = PxCreateCollection(); // collection for all the shared objects
        actorCollection  = PxCreateCollection(); // collection for all the nonshared objects

        sharedCollection->add(ref *shape);
        PxSerialization.complete(ref *sharedCollection, ref sr);          // chases the pointer from shape to material, and adds it
        PxSerialization.createSerialObjectIds(ref *sharedCollection, 77); // arbitrary choice of base for references to shared objects

        actorCollection->add(ref *prevActor);
        PxSerialization.complete(ref *actorCollection, ref sr, sharedCollection, true); // chases all pointers and recursively adds actors and joints
    }
Beispiel #3
0
    static void createDynamics()
    {
        const uint NbX = 8;
        const uint NbY = 8;

        PxVec3      dims          = new(0.2f, 0.1f, 0.2f);
        const float sphereRadius  = 0.2f;
        const float capsuleRadius = 0.2f;
        const float halfHeight    = 0.5f;

        const uint  NbLayers = 3;
        const float YScale   = 0.4f;
        const float YStart   = 6.0f;
        PxShape *   boxShape = gPhysics->createShape(new PxBoxGeometry(dims), *gMaterial,
                                                     false, PxShapeFlags.eVISUALIZATION | PxShapeFlags.eSCENE_QUERY_SHAPE | PxShapeFlags.eSIMULATION_SHAPE);  //BIOQUIRK: Missing default arguments
        PxShape *sphereShape = gPhysics->createShape(new PxSphereGeometry(sphereRadius), *gMaterial,
                                                     false, PxShapeFlags.eVISUALIZATION | PxShapeFlags.eSCENE_QUERY_SHAPE | PxShapeFlags.eSIMULATION_SHAPE);  //BIOQUIRK: Missing default arguments
        PxShape *capsuleShape = gPhysics->createShape(new PxCapsuleGeometry(capsuleRadius, halfHeight), *gMaterial,
                                                      false, PxShapeFlags.eVISUALIZATION | PxShapeFlags.eSCENE_QUERY_SHAPE | PxShapeFlags.eSIMULATION_SHAPE); //BIOQUIRK: Missing default arguments
        PxMat33 m;

        for (uint j = 0; j < NbLayers; j++)
        {
            float  angle = (float)j * 0.08f;
            PxQuat rot   = setRotY(out m, angle);

            const float ScaleX = 4.0f;
            const float ScaleY = 4.0f;

            for (uint y = 0; y < NbY; y++)
            {
                for (uint x = 0; x < NbX; x++)
                {
                    float xf = ((float)(x) - (float)(NbX) * 0.5f) * ScaleX;
                    float yf = ((float)(y) - (float)(NbY) * 0.5f) * ScaleY;

                    PxRigidDynamic *dynamic = null;

                    uint   v   = j & 3;
                    PxVec3 pos = new PxVec3(xf, YStart + (float)(j) * YScale, yf);

                    switch (v)
                    {
                    case 0:
                    {
                        PxTransform pose = new(pos, rot);
                        dynamic = gPhysics->createRigidDynamic(pose);
                        dynamic->attachShape(ref *boxShape);
                        break;
                    }

                    case 1:
                    {
                        PxTransform pose = new(pos, new PxQuat(PxIdentity));
                        dynamic = gPhysics->createRigidDynamic(pose);
                        dynamic->attachShape(ref *sphereShape);
                        break;
                    }

                    default:
                    {
                        PxTransform pose = new(pos, rot);
                        dynamic = gPhysics->createRigidDynamic(pose);
                        dynamic->attachShape(ref *capsuleShape);
                        break;
                    }
                    }
                    ;

                    PxRigidBodyExt.updateMassAndInertia(ref *dynamic, 10f);

                    gScene->addActor(ref *dynamic);
                }
            }
        }
    }
Beispiel #4
0
        public static void Main(string[] args)
        {
            Console.WriteLine($"PhysX native runtime build information: '{MochiPhysX.BuildInfo}'...");

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Initializing error callback");
            // Switch between these to use PhysX's default error callback or one implemented from C#
            PxDefaultErrorCallback errorCallback = new();

            //PxErrorCallback errorCallback = ErrorCallback.Create();

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Initializing allocator callback");
            // Switch between these to use PhysX's default allocator callback or one implemented from C#
            PxDefaultAllocator allocator = new();

            //PxAllocatorCallback allocator = BasicAllocator.Create();
            //PxAllocatorCallback allocator = LoggingAllocator.Create();

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Initializing foundation");
            //BIOQUIRK: PhysX owns both of these references, which means both the allocator and error callback must remain pinned for the lifetime of the foundation.
            // (In our case they're stack allocated and implicitly pinned.)
            // This seems somewhat unobvious since C# references don't normally care. Should we emit this function differently to convey the unsafe-ness here?
            PxFoundation *foundation = PxCreateFoundation(PX_PHYSICS_VERSION, ref allocator, ref errorCallback);

            if (foundation == null)
            {
                Console.Error.WriteLine("Failed to create foundation.");
                return;
            }

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Initializing Pvd...");
            //BIOQUIRK: This pattern comes up a lot in PhysX. It does technically match how it is in C++ though, not sure if it's a problem.
            // I experimentally enabled having C++ reference returns translate as C# reference returns, but that creates a weird situation when you need to store them.
            PxPvd *pvd = PxCreatePvd(ref *foundation);

            PxPvdTransport *transport;

            byte[] host = Encoding.ASCII.GetBytes("127.0.0.1");
            fixed(byte *hostP = host)
            {
                transport = PxDefaultPvdSocketTransportCreate(hostP, 5425, 10);
            }

            Console.WriteLine("Connecting to Pvd...");
            pvd->connect(ref *transport, PxPvdInstrumentationFlags.eALL);

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Initializing physics");
            PxPhysics *physics = PxCreatePhysics(PX_PHYSICS_VERSION, ref *foundation, new PxTolerancesScale(), trackOutstandingAllocations: true, pvd);

            if (physics == null)
            {
                Console.Error.WriteLine("Failed to create physics.");
                return;
            }

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Creating dispatcher");
            PxDefaultCpuDispatcher *dispatcher = PxDefaultCpuDispatcherCreate(2, null);

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Creating scene");
            PxSceneDesc sceneDescription = new(*physics->getTolerancesScale());

            sceneDescription.gravity = new PxVec3()
            {
                x = 0f, y = -9.81f, z = 0f
            };
            sceneDescription.cpuDispatcher = (PxCpuDispatcher *)dispatcher;
            sceneDescription.filterShader  = PxDefaultSimulationFilterShader;
            PxScene *scene = physics->createScene(sceneDescription);

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Configuring scene Pvd client");
            PxPvdSceneClient *pvdClient = scene->getScenePvdClient();

            if (pvdClient != null)
            {
                pvdClient->setScenePvdFlag(PxPvdSceneFlags.eTRANSMIT_CONSTRAINTS, true);
                pvdClient->setScenePvdFlag(PxPvdSceneFlags.eTRANSMIT_CONTACTS, true);
                pvdClient->setScenePvdFlag(PxPvdSceneFlags.eTRANSMIT_SCENEQUERIES, true);
            }

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Creating a basic material");
            PxMaterial *material = physics->createMaterial(0.5f, 0.5f, 0.6f);

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Adding a ground plane");
            PxPlane planeDescription = new PxPlane()
            {
                n = new PxVec3()
                {
                    x = 0f,
                    y = 1f,
                    z = 0f
                },
                d = 0f
            };
            PxRigidStatic *groundPlane = PxCreatePlane(ref *physics, planeDescription, ref *material);

            scene->addActor(ref *groundPlane, null);

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Adding stacks");
            {
                const float   halfExtent       = 2f;
                PxBoxGeometry stackBoxGeometry = new PxBoxGeometry(halfExtent, halfExtent, halfExtent);
                PxShapeFlags  shapeFlags       = PxShapeFlags.eVISUALIZATION | PxShapeFlags.eSCENE_QUERY_SHAPE | PxShapeFlags.eSIMULATION_SHAPE;
                //BIOQUIRK: shapeFlags should be able to be defaulted now but it isn't.
                PxShape *shape  = physics->createShape(stackBoxGeometry, *material, isExclusive: false, shapeFlags);
                float    stackZ = 10f;
                for (int stackNum = 0; stackNum < 5; stackNum++)
                {
                    const int size = 10;

                    PxVec3 transformPosition = new PxVec3()
                    {
                        x = 0f,
                        y = 0f,
                        z = stackZ -= 10f
                    };
                    PxTransform transform = new(transformPosition);

                    for (int i = 0; i < size; i++)
                    {
                        for (int j = 0; j < size - i; j++)
                        {
                            PxVec3 position = new PxVec3()
                            {
                                x = ((float)(j * 2) - (float)(size - i)) * halfExtent,
                                y = ((float)(i * 2 + 1)) * halfExtent,
                                z = 0f
                            };
                            PxTransform localTransform = new(position);

                            PxTransform     bodyTransform = transform.transform(localTransform);
                            PxRigidDynamic *body          = physics->createRigidDynamic(bodyTransform);
                            body->attachShape(ref *shape);
                            PxRigidBodyExt.updateMassAndInertia(ref *body, 10f);
                            scene->addActor(ref *body);
                        }
                    }
                }
                shape->release();
            }

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Throwing a ball at the stacks");
            {
                PxVec3 position = new PxVec3()
                {
                    x = 0f,
                    y = 40f,
                    z = 100f,
                };
                PxTransform      transform = new(position);
                PxSphereGeometry geometry  = new(10f);

                PxVec3 velocity = new PxVec3()
                {
                    x = 0f,
                    y = -50f,
                    z = -100f
                };

                PxTransform     identity = new(default(PxIDENTITY)); //BIOQUIRK: This could be a special generated property instead. Also missing default.
                PxRigidDynamic *dynamic  = PxCreateDynamic(ref *physics, transform, geometry, ref *material, 10f, identity);
                dynamic->setAngularDamping(0.5f);
                dynamic->setLinearVelocity(velocity);
                scene->addActor(ref *dynamic);
            }

            //---------------------------------------------------------------------------------------------------------------------------------------
            const int noInputFrameCount = 100;

            Console.WriteLine($"Simulating the world{(Console.IsInputRedirected ? $" for {noInputFrameCount} frames." : "... (Press escape to stop.)")}");
            Stopwatch sw       = new Stopwatch();
            int       frameNum = 0;

            const uint scratchMemoryBlockSize  = 16 * 1024;
            const uint scratchMemoryBlockCount = 4;
            uint       scratchMemorySize       = scratchMemoryBlockSize * scratchMemoryBlockCount;
            void *     scratchMemory           = allocator.allocate(scratchMemorySize, null, null, 0);

            while (true)
            {
                double msSinceLastTick = sw.Elapsed.TotalMilliseconds;
                string consoleTitle    = $"Simulating frame {frameNum} -- {msSinceLastTick:0.00} ms -- {1.0 / (msSinceLastTick / 1000.0):00.0} FPS";
                if (BasicAllocator.AllocationCount > 0) // This is only applicable when a allocator implemented in C# is in use, assume 0 allocations implies the PhysX one is being used
                {
                    consoleTitle += $" -- {BasicAllocator.AllocationCount} allocations";
                    BasicAllocator.AllocationCount = 0;
                }
                Console.Title = consoleTitle;
                frameNum++;

                sw.Restart();

                scene->simulate(1f / 60f, scratchMemBlock: scratchMemory, scratchMemBlockSize: scratchMemorySize);

                uint errors;
                scene->fetchResults(true, &errors);
                if (errors != 0)
                {
                    Console.WriteLine($"fetchResults error: {errors}");
                }

                if (Console.IsInputRedirected)
                {
                    if (frameNum > noInputFrameCount)
                    {
                        break;
                    }
                }
                else if (Console.KeyAvailable && Console.ReadKey(true).Key == ConsoleKey.Escape)
                {
                    break;
                }
            }

            //---------------------------------------------------------------------------------------------------------------------------------------
            Console.WriteLine("Shutting down");
            allocator.deallocate(scratchMemory);
            physics->release();
            foundation->release();
        }
    static void createScissorLift()
    {
        const float runnerLength      = 2f;
        const float placementDistance = 1.8f;

        const float cosAng = (placementDistance) / (runnerLength);

        float angle = PxAcos(cosAng);

        float sinAng = PxSin(angle);

        PxQuat leftRot  = new(-angle, new PxVec3(1f, 0f, 0f));
        PxQuat rightRot = new(angle, new PxVec3(1f, 0f, 0f));

        //(1) Create base...
        PxArticulationLink * @base             = gArticulation->createLink(null, new PxTransform(new PxVec3(0f, 0.25f, 0f)));
        const PxShapeFlags   defaultShapeFlags = PxShapeFlags.eVISUALIZATION | PxShapeFlags.eSCENE_QUERY_SHAPE | PxShapeFlags.eSIMULATION_SHAPE; //BIOQUIRK: Many missing defaults

        PxRigidActorExt.createExclusiveShape(ref *@base, new PxBoxGeometry(0.5f, 0.25f, 1.5f), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *@base, 3f);

        //Now create the slider and fixed joints...

        gArticulation->setSolverIterationCounts(32);

        PxArticulationLink *leftRoot = gArticulation->createLink(@base, new PxTransform(new PxVec3(0f, 0.55f, -0.9f)));

        PxRigidActorExt.createExclusiveShape(ref *leftRoot, new PxBoxGeometry(0.5f, 0.05f, 0.05f), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *leftRoot, 1f);

        PxArticulationLink *rightRoot = gArticulation->createLink(@base, new PxTransform(new PxVec3(0f, 0.55f, 0.9f)));

        PxRigidActorExt.createExclusiveShape(ref *rightRoot, new PxBoxGeometry(0.5f, 0.05f, 0.05f), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *rightRoot, 1f);

        PxArticulationJointReducedCoordinate *joint = static_cast <PxArticulationJointReducedCoordinate>(leftRoot->getInboundJoint());

        joint->setJointType(PxArticulationJointType.eFIX);
        joint->setParentPose(new PxTransform(new PxVec3(0f, 0.25f, -0.9f)));
        joint->setChildPose(new PxTransform(new PxVec3(0f, -0.05f, 0f)));

        //Set up the drive joint...
        gDriveJoint = static_cast <PxArticulationJointReducedCoordinate>(rightRoot->getInboundJoint());
        gDriveJoint->setJointType(PxArticulationJointType.ePRISMATIC);
        gDriveJoint->setMotion(PxArticulationAxis.eZ, PxArticulationMotions.eLIMITED);
        gDriveJoint->setLimit(PxArticulationAxis.eZ, -1.4f, 0.2f);
        gDriveJoint->setDrive(PxArticulationAxis.eZ, 100000f, 0f, float.MaxValue);

        gDriveJoint->setParentPose(new PxTransform(new PxVec3(0f, 0.25f, 0.9f)));
        gDriveJoint->setChildPose(new PxTransform(new PxVec3(0f, -0.05f, 0f)));


        const uint          linkHeight = 3;
        PxArticulationLink *currLeft = leftRoot, currRight = rightRoot;

        PxQuat rightParentRot = new(PxIdentity);
        PxQuat leftParentRot  = new(PxIdentity);

        for (uint i = 0; i < linkHeight; ++i)
        {
            PxVec3 pos = new(0.5f, 0.55f + 0.1f * (1 + i), 0f);
            PxArticulationLink *leftLink = gArticulation->createLink(currLeft, new PxTransform(pos.operator_Plus(new PxVec3(0f, sinAng * (2 * i + 1), 0f)), leftRot)); //BIOQUIRK: Operator overload
            PxRigidActorExt.createExclusiveShape(ref *leftLink, new PxBoxGeometry(0.05f, 0.05f, 1f), *gMaterial, defaultShapeFlags);
            PxRigidBodyExt.updateMassAndInertia(ref *leftLink, 1f);

            PxVec3 leftAnchorLocation = pos.operator_Plus(new PxVec3(0f, sinAng * (2 * i), -0.9f)); //BIOQUIRK: Operator overload

            joint = static_cast <PxArticulationJointReducedCoordinate>(leftLink->getInboundJoint());
            joint->setParentPose(new PxTransform(currLeft->getGlobalPose().transformInv(leftAnchorLocation), leftParentRot));
            joint->setChildPose(new PxTransform(new PxVec3(0f, 0f, -1f), rightRot));
            joint->setJointType(PxArticulationJointType.eREVOLUTE);

            leftParentRot = leftRot;

            joint->setMotion(PxArticulationAxis.eTWIST, PxArticulationMotions.eLIMITED);
            joint->setLimit(PxArticulationAxis.eTWIST, -MathF.PI, angle);


            PxArticulationLink *rightLink = gArticulation->createLink(currRight, new PxTransform(pos.operator_Plus(new PxVec3(0f, sinAng * (2 * i + 1), 0f)), rightRot)); //BIOQUIRK: Operator overload
            PxRigidActorExt.createExclusiveShape(ref *rightLink, new PxBoxGeometry(0.05f, 0.05f, 1f), *gMaterial, defaultShapeFlags);
            PxRigidBodyExt.updateMassAndInertia(ref *rightLink, 1f);

            PxVec3 rightAnchorLocation = pos.operator_Plus(new PxVec3(0f, sinAng * (2 * i), 0.9f)); //BIOQUIRK: Operator overload

            joint = static_cast <PxArticulationJointReducedCoordinate>(rightLink->getInboundJoint());
            joint->setJointType(PxArticulationJointType.eREVOLUTE);
            joint->setParentPose(new PxTransform(currRight->getGlobalPose().transformInv(rightAnchorLocation), rightParentRot));
            joint->setChildPose(new PxTransform(new PxVec3(0f, 0f, 1f), leftRot));
            joint->setMotion(PxArticulationAxis.eTWIST, PxArticulationMotions.eLIMITED);
            joint->setLimit(PxArticulationAxis.eTWIST, -angle, MathF.PI);

            rightParentRot = rightRot;

            PxD6Joint *d6joint = PxD6JointCreate(ref *gPhysics, leftLink, new PxTransform(PxIdentity), rightLink, new PxTransform(PxIdentity));

            d6joint->setMotion(PxD6Axis.eTWIST, PxD6Motion.eFREE);
            d6joint->setMotion(PxD6Axis.eSWING2, PxD6Motion.eFREE);
            d6joint->setMotion(PxD6Axis.eSWING1, PxD6Motion.eFREE);

            currLeft  = rightLink;
            currRight = leftLink;
        }


        PxArticulationLink *leftTop = gArticulation->createLink(currLeft, currLeft->getGlobalPose().transform(new PxTransform(new PxVec3(-0.5f, 0f, -1.0f), leftParentRot)));

        PxRigidActorExt.createExclusiveShape(ref *leftTop, new PxBoxGeometry(0.5f, 0.05f, 0.05f), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *leftTop, 1f);

        PxArticulationLink *rightTop = gArticulation->createLink(currRight, currRight->getGlobalPose().transform(new PxTransform(new PxVec3(-0.5f, 0f, 1.0f), rightParentRot)));

        PxRigidActorExt.createExclusiveShape(ref *rightTop, new PxCapsuleGeometry(0.05f, 0.8f), *gMaterial, defaultShapeFlags);
        //PxRigidActorExt.createExclusiveShape(ref *rightTop, PxBoxGeometry(0.5f, 0.05f, 0.05f), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *rightTop, 1f);

        joint = static_cast <PxArticulationJointReducedCoordinate>(leftTop->getInboundJoint());
        joint->setParentPose(new PxTransform(new PxVec3(0f, 0f, -1f), currLeft->getGlobalPose().q.getConjugate()));
        joint->setChildPose(new PxTransform(new PxVec3(0.5f, 0f, 0f), leftTop->getGlobalPose().q.getConjugate()));
        joint->setJointType(PxArticulationJointType.eREVOLUTE);
        joint->setMotion(PxArticulationAxis.eTWIST, PxArticulationMotions.eFREE);
        //joint->setDrive(PxArticulationAxis.eTWIST, 0f, 10f, float.MaxValue);

        joint = static_cast <PxArticulationJointReducedCoordinate>(rightTop->getInboundJoint());
        joint->setParentPose(new PxTransform(new PxVec3(0f, 0f, 1f), currRight->getGlobalPose().q.getConjugate()));
        joint->setChildPose(new PxTransform(new PxVec3(0.5f, 0f, 0f), rightTop->getGlobalPose().q.getConjugate()));
        joint->setJointType(PxArticulationJointType.eREVOLUTE);
        joint->setMotion(PxArticulationAxis.eTWIST, PxArticulationMotions.eFREE);
        //joint->setDrive(PxArticulationAxis.eTWIST, 0f, 10f, float.MaxValue);


        currLeft  = leftRoot;
        currRight = rightRoot;

        rightParentRot = new PxQuat(PxIdentity);
        leftParentRot  = new PxQuat(PxIdentity);

        for (uint i = 0; i < linkHeight; ++i)
        {
            PxVec3 pos = new(-0.5f, 0.55f + 0.1f * (1 + i), 0f);
            PxArticulationLink *leftLink = gArticulation->createLink(currLeft, new PxTransform(pos.operator_Plus(new PxVec3(0f, sinAng * (2 * i + 1), 0f)), leftRot)); //BIOQURK: Operator overload
            PxRigidActorExt.createExclusiveShape(ref *leftLink, new PxBoxGeometry(0.05f, 0.05f, 1f), *gMaterial, defaultShapeFlags);
            PxRigidBodyExt.updateMassAndInertia(ref *leftLink, 1f);

            PxVec3 leftAnchorLocation = pos.operator_Plus(new PxVec3(0f, sinAng * (2 * i), -0.9f)); //BIOQUIRK: Operator overload

            joint = static_cast <PxArticulationJointReducedCoordinate>(leftLink->getInboundJoint());
            joint->setJointType(PxArticulationJointType.eREVOLUTE);
            joint->setParentPose(new PxTransform(currLeft->getGlobalPose().transformInv(leftAnchorLocation), leftParentRot));
            joint->setChildPose(new PxTransform(new PxVec3(0f, 0f, -1f), rightRot));

            leftParentRot = leftRot;

            joint->setMotion(PxArticulationAxis.eTWIST, PxArticulationMotions.eLIMITED);
            joint->setLimit(PxArticulationAxis.eTWIST, -float.MaxValue, angle);

            PxArticulationLink *rightLink = gArticulation->createLink(currRight, new PxTransform(pos.operator_Plus(new PxVec3(0f, sinAng * (2 * i + 1), 0f)), rightRot)); //BIOQUIRK: Operator overload
            PxRigidActorExt.createExclusiveShape(ref *rightLink, new PxBoxGeometry(0.05f, 0.05f, 1f), *gMaterial, defaultShapeFlags);
            PxRigidBodyExt.updateMassAndInertia(ref *rightLink, 1f);

            PxVec3 rightAnchorLocation = pos.operator_Plus(new PxVec3(0f, sinAng * (2 * i), 0.9f)); //BIOQUIRK: Operator overload

            /*joint = PxD6JointCreate(ref *getPhysics(), currRight, new PxTransform(currRight->getGlobalPose().transformInv(rightAnchorLocation)),
             * rightLink, new PxTransform(new PxVec3(0f, 0f, 1f)));*/

            joint = static_cast <PxArticulationJointReducedCoordinate>(rightLink->getInboundJoint());
            joint->setParentPose(new PxTransform(currRight->getGlobalPose().transformInv(rightAnchorLocation), rightParentRot));
            joint->setJointType(PxArticulationJointType.eREVOLUTE);
            joint->setChildPose(new PxTransform(new PxVec3(0f, 0f, 1f), leftRot));
            joint->setMotion(PxArticulationAxis.eTWIST, PxArticulationMotions.eLIMITED);
            joint->setLimit(PxArticulationAxis.eTWIST, -angle, float.MaxValue);

            rightParentRot = rightRot;

            PxD6Joint *d6joint = PxD6JointCreate(ref *gPhysics, leftLink, new PxTransform(PxIdentity), rightLink, new PxTransform(PxIdentity));

            d6joint->setMotion(PxD6Axis.eTWIST, PxD6Motion.eFREE);
            d6joint->setMotion(PxD6Axis.eSWING1, PxD6Motion.eFREE);
            d6joint->setMotion(PxD6Axis.eSWING2, PxD6Motion.eFREE);

            currLeft  = rightLink;
            currRight = leftLink;
        }

        {
            PxD6Joint *d6joint = PxD6JointCreate(ref *gPhysics, currLeft, new PxTransform(new PxVec3(0f, 0f, -1f)), leftTop, new PxTransform(new PxVec3(-0.5f, 0f, 0f)));

            d6joint->setMotion(PxD6Axis.eTWIST, PxD6Motion.eFREE);
            d6joint->setMotion(PxD6Axis.eSWING1, PxD6Motion.eFREE);
            d6joint->setMotion(PxD6Axis.eSWING2, PxD6Motion.eFREE);

            d6joint = PxD6JointCreate(ref *gPhysics, currRight, new PxTransform(new PxVec3(0f, 0f, 1f)), rightTop, new PxTransform(new PxVec3(-0.5f, 0f, 0f)));

            d6joint->setMotion(PxD6Axis.eTWIST, PxD6Motion.eFREE);
            d6joint->setMotion(PxD6Axis.eSWING1, PxD6Motion.eFREE);
            d6joint->setMotion(PxD6Axis.eSWING2, PxD6Motion.eFREE);
        }

        PxTransform topPose = new(new PxVec3(0f, leftTop->getGlobalPose().p.y + 0.15f, 0f));

        PxArticulationLink *top = gArticulation->createLink(leftTop, topPose);

        PxRigidActorExt.createExclusiveShape(ref *top, new PxBoxGeometry(0.5f, 0.1f, 1.5f), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *top, 1f);

        joint = static_cast <PxArticulationJointReducedCoordinate>(top->getInboundJoint());
        joint->setJointType(PxArticulationJointType.eFIX);
        joint->setParentPose(new PxTransform(new PxVec3(0f, 0.0f, 0f)));
        joint->setChildPose(new PxTransform(new PxVec3(0f, -0.15f, -0.9f)));

        gScene->addArticulation(ref *gArticulation);

        for (uint i = 0; i < gArticulation->getNbLinks(); ++i)
        {
            PxArticulationLink *link;
            gArticulation->getLinks(&link, 1, i);

            link->setLinearDamping(0.2f);
            link->setAngularDamping(0.2f);

            link->setMaxAngularVelocity(20f);
            link->setMaxLinearVelocity(100f);

            if (link != top)
            {
                for (uint b = 0; b < link->getNbShapes(); ++b)
                {
                    PxShape *shape;
                    link->getShapes(&shape, 1, b);

                    shape->setSimulationFilterData(new PxFilterData(0, 0, 1, 0));
                }
            }
        }

        PxVec3      halfExt = new(0.25f);
        const float density = 0.5f;

        PxRigidDynamic *box0 = gPhysics->createRigidDynamic(new PxTransform(new PxVec3(-0.25f, 5f, 0.5f)));

        PxRigidActorExt.createExclusiveShape(ref *box0, new PxBoxGeometry(halfExt), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *box0, density);

        gScene->addActor(ref *box0);

        PxRigidDynamic *box1 = gPhysics->createRigidDynamic(new PxTransform(new PxVec3(0.25f, 5f, 0.5f)));

        PxRigidActorExt.createExclusiveShape(ref *box1, new PxBoxGeometry(halfExt), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *box1, density);

        gScene->addActor(ref *box1);

        PxRigidDynamic *box2 = gPhysics->createRigidDynamic(new PxTransform(new PxVec3(-0.25f, 4.5f, 0.5f)));

        PxRigidActorExt.createExclusiveShape(ref *box2, new PxBoxGeometry(halfExt), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *box2, density);

        gScene->addActor(ref *box2);

        PxRigidDynamic *box3 = gPhysics->createRigidDynamic(new PxTransform(new PxVec3(0.25f, 4.5f, 0.5f)));

        PxRigidActorExt.createExclusiveShape(ref *box3, new PxBoxGeometry(halfExt), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *box3, density);

        gScene->addActor(ref *box3);

        PxRigidDynamic *box4 = gPhysics->createRigidDynamic(new PxTransform(new PxVec3(-0.25f, 5f, 0f)));

        PxRigidActorExt.createExclusiveShape(ref *box4, new PxBoxGeometry(halfExt), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *box4, density);

        gScene->addActor(ref *box4);

        PxRigidDynamic *box5 = gPhysics->createRigidDynamic(new PxTransform(new PxVec3(0.25f, 5f, 0f)));

        PxRigidActorExt.createExclusiveShape(ref *box5, new PxBoxGeometry(halfExt), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *box5, density);

        gScene->addActor(ref *box5);

        PxRigidDynamic *box6 = gPhysics->createRigidDynamic(new PxTransform(new PxVec3(-0.25f, 4.5f, 0f)));

        PxRigidActorExt.createExclusiveShape(ref *box6, new PxBoxGeometry(halfExt), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *box6, density);

        gScene->addActor(ref *box6);

        PxRigidDynamic *box7 = gPhysics->createRigidDynamic(new PxTransform(new PxVec3(0.25f, 4.5f, 0f)));

        PxRigidActorExt.createExclusiveShape(ref *box7, new PxBoxGeometry(halfExt), *gMaterial, defaultShapeFlags);
        PxRigidBodyExt.updateMassAndInertia(ref *box7, density);

        gScene->addActor(ref *box7);
    }
    static void createLongChain()
    {
        const float scale       = 0.25f;
        const float radius      = 0.5f * scale;
        const float halfHeight  = 1.0f * scale;
        const uint  nbCapsules  = 40;
        const float capsuleMass = 1.0f;

        PxVec3   initPos      = new(0.0f, 24.0f, 0.0f);
        PxVec3   pos          = initPos;
        PxShape *capsuleShape = gPhysics->createShape(new PxCapsuleGeometry(radius, halfHeight), *gMaterial,
                                                      false, PxShapeFlags.eVISUALIZATION | PxShapeFlags.eSCENE_QUERY_SHAPE | PxShapeFlags.eSIMULATION_SHAPE); //BIOQUIRK: Missing defaults
        PxArticulationLink *firstLink = null;
        PxArticulationLink *parent    = null;

        const bool overlappingLinks = true; // Change this for another kind of rope

        gArticulation->setSolverIterationCounts(16);

        // Create rope
        for (uint i = 0; i < nbCapsules; i++)
        {
            PxArticulationLink *link = gArticulation->createLink(parent, new PxTransform(pos));
            if (firstLink == null)
            {
                firstLink = link;
            }

            link->attachShape(ref *capsuleShape);
            PxRigidBodyExt.setMassAndUpdateInertia(ref *link, capsuleMass);

            link->setLinearDamping(0.1f);
            link->setAngularDamping(0.1f);

            link->setMaxAngularVelocity(30f);
            link->setMaxLinearVelocity(100f);

            PxArticulationJointBase *joint = link->getInboundJoint();

            if (joint != null) // Will be null for root link
            {
#if USE_REDUCED_COORDINATE_ARTICULATION
                PxArticulationJointReducedCoordinate *rcJoint = static_cast <PxArticulationJointReducedCoordinate>(joint);
                rcJoint->setJointType(PxArticulationJointType.eSPHERICAL);
                rcJoint->setMotion(PxArticulationAxis.eSWING2, PxArticulationMotions.eFREE);
                rcJoint->setMotion(PxArticulationAxis.eSWING1, PxArticulationMotions.eFREE);
                rcJoint->setMotion(PxArticulationAxis.eTWIST, PxArticulationMotions.eFREE);
                rcJoint->setFrictionCoefficient(1f);
                rcJoint->setMaxJointVelocity(1000000f);
#endif
                if (overlappingLinks)
                {
                    joint->setParentPose(new PxTransform(new PxVec3(halfHeight, 0.0f, 0.0f)));
                    joint->setChildPose(new PxTransform(new PxVec3(-halfHeight, 0.0f, 0.0f)));
                }
                else
                {
                    joint->setParentPose(new PxTransform(new PxVec3(radius + halfHeight, 0.0f, 0.0f)));
                    joint->setChildPose(new PxTransform(new PxVec3(-radius - halfHeight, 0.0f, 0.0f)));
                }
            }

            if (overlappingLinks)
            {
                pos.x += (radius + halfHeight * 2.0f);
            }
            else
            {
                pos.x += (radius + halfHeight) * 2.0f;
            }
            parent = link;
        }

        //Attach large & heavy box at the end of the rope
        {
            const float boxMass  = 50.0f;
            const float boxSize  = 1.0f;
            PxShape *   boxShape = gPhysics->createShape(new PxBoxGeometry(boxSize, boxSize, boxSize), *gMaterial,
                                                         false, PxShapeFlags.eVISUALIZATION | PxShapeFlags.eSCENE_QUERY_SHAPE | PxShapeFlags.eSIMULATION_SHAPE); //BIOQUIRK: Missing defaults

            pos.x -= (radius + halfHeight) * 2.0f;
            pos.x += (radius + halfHeight) + boxSize;

            PxArticulationLink *link = gArticulation->createLink(parent, new PxTransform(pos));

            link->setLinearDamping(0.1f);
            link->setAngularDamping(0.1f);
            link->setMaxAngularVelocity(30f);
            link->setMaxLinearVelocity(100f);

            link->attachShape(ref *boxShape);
            PxRigidBodyExt.setMassAndUpdateInertia(ref *link, boxMass);

            PxArticulationJointBase *joint = link->getInboundJoint();
#if USE_REDUCED_COORDINATE_ARTICULATION
            PxArticulationJointReducedCoordinate *rcJoint = static_cast <PxArticulationJointReducedCoordinate>(joint);
            rcJoint->setJointType(PxArticulationJointType.eSPHERICAL);
            rcJoint->setMotion(PxArticulationAxis.eSWING2, PxArticulationMotions.eFREE);
            rcJoint->setMotion(PxArticulationAxis.eSWING1, PxArticulationMotions.eFREE);
            rcJoint->setMotion(PxArticulationAxis.eTWIST, PxArticulationMotions.eFREE);
            rcJoint->setFrictionCoefficient(1f);
            rcJoint->setMaxJointVelocity(1000000f);
#endif
            if (joint != null) // Will be null for root link
            {
                joint->setParentPose(new PxTransform(new PxVec3(radius + halfHeight, 0.0f, 0.0f)));
                joint->setChildPose(new PxTransform(new PxVec3(-boxSize, 0.0f, 0.0f)));
            }
        }
        gScene->addArticulation(ref *gArticulation);

#if USE_REDUCED_COORDINATE_ARTICULATION
        gArticulation->setArticulationFlags(PxArticulationFlags.eFIX_BASE);
#else
        // Attach articulation to static world
        {
            PxShape *anchorShape = gPhysics->createShape(new PxSphereGeometry(0.05f), *gMaterial,
                                                         false, PxShapeFlags.eVISUALIZATION | PxShapeFlags.eSCENE_QUERY_SHAPE | PxShapeFlags.eSIMULATION_SHAPE); //BIOQUIRK: Missing defaults
            PxRigidStatic *anchor = PxCreateStatic(ref *gPhysics, new PxTransform(initPos), ref *anchorShape);
            gScene->addActor(ref *anchor);
            PxSphericalJoint *j = PxSphericalJointCreate(ref *gPhysics, anchor, new PxTransform(new PxVec3(0.0f)), firstLink, new PxTransform(new PxVec3(0.0f)));
        }
#endif

        // Create obstacle
        {
            PxShape *boxShape = gPhysics->createShape(new PxBoxGeometry(1.0f, 0.1f, 2.0f), *gMaterial,
                                                      false, PxShapeFlags.eVISUALIZATION | PxShapeFlags.eSCENE_QUERY_SHAPE | PxShapeFlags.eSIMULATION_SHAPE); //BIOQUIRK: Missing defaults
            PxRigidStatic *obstacle = PxCreateStatic(ref *gPhysics, new PxTransform(initPos.operator_Plus(new PxVec3(10.0f, -3.0f, 0.0f))), ref *boxShape);   //BIOQUIRK: Overloaded operator
            gScene->addActor(ref *obstacle);
        }
    }