PxVehiclePvdComponents.h

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2022 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
 
#pragma once
 
#include "vehicle2/PxVehicleComponent.h"
#include "vehicle2/PxVehicleParams.h"
#include "PxVehiclePvdFunctions.h"
 
#if !PX_DOXYGEN
namespace physx
{
namespace vehicle2
{
#endif
 
#define VEHICLE_FLOAT_ARRAY_DATA(b)  PxVehicleArrayData<PxReal> b; b.setEmpty();
#define VEHICLE_ARRAY_DATA(a, b)  PxVehicleArrayData<const a> b; b.setEmpty();
#define VEHICLE_SIZED_ARRAY_DATA(a, b)  PxVehicleSizedArrayData<const a> b; b.setEmpty();
 
class PxVehiclePVDComponent : public PxVehicleComponent
{
public:
    
    PxVehiclePVDComponent() : PxVehicleComponent() , firstTime(true) {}
    virtual ~PxVehiclePVDComponent() {}
 
    virtual void getDataForPVDComponent(
        const PxVehicleAxleDescription*& axleDescription, 
        const PxVehicleRigidBodyParams*& rbodyParams, const PxVehicleRigidBodyState*& rbodyState,
        const PxVehicleSuspensionStateCalculationParams*& suspStateCalcParams,
        PxVehicleSizedArrayData<const PxVehicleBrakeCommandResponseParams>& brakeResponseParams,
        const PxVehicleSteerCommandResponseParams*& steerResponseParams, 
        PxVehicleArrayData<PxReal>& brakeResponseStates,
        PxVehicleArrayData<PxReal>& steerResponseStates, 
        PxVehicleArrayData<const PxVehicleWheelParams>& wheelParams,
        PxVehicleArrayData<const PxVehicleWheelActuationState>& wheelActuationStates,
        PxVehicleArrayData<const PxVehicleWheelRigidBody1dState>& wheelRigidBody1dStates,
        PxVehicleArrayData<const PxVehicleWheelLocalPose>& wheelLocalPoses,
        PxVehicleArrayData<const PxVehicleRoadGeometryState>& roadGeomStates,
        PxVehicleArrayData<const PxVehicleSuspensionParams>& suspParams,
        PxVehicleArrayData<const PxVehicleSuspensionComplianceParams>& suspCompParams,
        PxVehicleArrayData<const PxVehicleSuspensionForceParams>& suspForceParams,
        PxVehicleArrayData<const PxVehicleSuspensionState>& suspStates,
        PxVehicleArrayData<const PxVehicleSuspensionComplianceState>& suspCompStates,
        PxVehicleArrayData<const PxVehicleSuspensionForce>& suspForces,
        PxVehicleArrayData<const PxVehicleTireForceParams>& tireForceParams,
        PxVehicleArrayData<const PxVehicleTireDirectionState>& tireDirectionStates,
        PxVehicleArrayData<const PxVehicleTireSpeedState>& tireSpeedStates,
        PxVehicleArrayData<const PxVehicleTireSlipState>& tireSlipStates,
        PxVehicleArrayData<const PxVehicleTireStickyState>& tireStickyStates,
        PxVehicleArrayData<const PxVehicleTireGripState>& tireGripStates,
        PxVehicleArrayData<const PxVehicleTireCamberAngleState>& tireCamberStates,
        PxVehicleArrayData<const PxVehicleTireForce>& tireForces, 
        PxVehicleSizedArrayData<const PxVehicleAntiRollForceParams>& antiRollForceParams,
        const PxVehicleAntiRollTorque*& antiRollTorque,
        const PxVehicleCommandState*& commandState, 
        const PxVehicleDirectDriveThrottleCommandResponseParams*& directDriveThrottleResponseParams,
        const PxVehicleDirectDriveTransmissionCommandState*& directDriveTransmissionState,
        PxVehicleArrayData<PxReal>& directDrivethrottleResponseState,
        const PxVehicleClutchCommandResponseParams*& clutchResponseParams,
        const PxVehicleClutchParams*& clutchParams,
        const PxVehicleEngineParams*& engineParams,
        const PxVehicleGearboxParams*& gearboxParams,
        const PxVehicleAutoboxParams*& autoboxParams,
        const PxVehicleMultiWheelDriveDifferentialParams*& multiWheelDiffParams,
        const PxVehicleFourWheelDriveDifferentialParams*& fourWheelDiffParams,
        const PxVehicleEngineDriveTransmissionCommandState*& engineDriveTransmissionState,
        const PxVehicleClutchCommandResponseState*& clutchResponseState,
        const PxVehicleEngineDriveThrottleCommandResponseState*& engineDriveThrottleResponseState,
        const PxVehicleEngineState*& engineState,
        const PxVehicleGearboxState*& gearboxState,
        const PxVehicleAutoboxState*& autoboxState,
        const PxVehicleDifferentialState*& diffState,
        const PxVehicleClutchSlipState*& clutchSlipState,
        PxVehicleArrayData<const PxVehiclePhysXSuspensionLimitConstraintParams>& physxConstraintParams,
        PxVehicleArrayData<const PxVehiclePhysXMaterialFrictionParams>& physxMaterialFrictionParams,
        const PxVehiclePhysXActor*& physxActor,
        const PxVehiclePhysXRoadGeometryQueryParams*& physxRoadGeomQryParams,
        PxVehicleArrayData<const PxVehiclePhysXRoadGeometryQueryState>& physxRoadGeomStates,
        PxVehicleArrayData<const PxVehiclePhysXConstraintState>& physxConstraintStates,
        PxVehiclePvdObjectHandles*& objectHandles) = 0;
 
    virtual bool update(const PxReal dt, const PxVehicleSimulationContext& context)
    { 
        PX_UNUSED(dt);
        PX_UNUSED(context);
 
        if(!context.pvdContext.attributeHandles || !context.pvdContext.writer)
            return true;
 
        const PxVehicleAxleDescription* axleDesc = NULL;
        const PxVehicleRigidBodyParams* rbodyParams = NULL;
        const PxVehicleRigidBodyState* rbodyState = NULL;
        const PxVehicleSuspensionStateCalculationParams* suspStateCalcParams = NULL;
        VEHICLE_SIZED_ARRAY_DATA(PxVehicleBrakeCommandResponseParams, brakeResponseParams);
        const PxVehicleSteerCommandResponseParams* steerResponseParams = NULL;
        VEHICLE_FLOAT_ARRAY_DATA(brakeResponseStates);
        VEHICLE_FLOAT_ARRAY_DATA(steerResponseStates);
        VEHICLE_ARRAY_DATA(PxVehicleWheelParams, wheelParams);
        VEHICLE_ARRAY_DATA(PxVehicleWheelActuationState, wheelActuationStates);
        VEHICLE_ARRAY_DATA(PxVehicleWheelRigidBody1dState, wheelRigidBody1dStates);
        VEHICLE_ARRAY_DATA(PxVehicleWheelLocalPose, wheelLocalPoses);
        VEHICLE_ARRAY_DATA(PxVehicleRoadGeometryState, roadGeomStates);
        VEHICLE_ARRAY_DATA(PxVehicleSuspensionParams, suspParams);
        VEHICLE_ARRAY_DATA(PxVehicleSuspensionComplianceParams, suspComplianceParams);
        VEHICLE_ARRAY_DATA(PxVehicleSuspensionForceParams, suspForceParams);
        VEHICLE_ARRAY_DATA(PxVehicleSuspensionState, suspStates);
        VEHICLE_ARRAY_DATA(PxVehicleSuspensionComplianceState, suspComplianceStates);
        VEHICLE_ARRAY_DATA(PxVehicleSuspensionForce, suspForces);
        VEHICLE_ARRAY_DATA(PxVehicleTireForceParams, tireForceParams);
        VEHICLE_ARRAY_DATA(PxVehicleTireDirectionState, tireDirectionStates);
        VEHICLE_ARRAY_DATA(PxVehicleTireSpeedState, tireSpeedStates);
        VEHICLE_ARRAY_DATA(PxVehicleTireSlipState, tireSlipStates);
        VEHICLE_ARRAY_DATA(PxVehicleTireStickyState, tireStickyStates);
        VEHICLE_ARRAY_DATA(PxVehicleTireGripState, tireGripStates);
        VEHICLE_ARRAY_DATA(PxVehicleTireCamberAngleState, tireCamberStates);
        VEHICLE_ARRAY_DATA(PxVehicleTireForce, tireForces);
        VEHICLE_SIZED_ARRAY_DATA(PxVehicleAntiRollForceParams, antiRollParams);
        const PxVehicleAntiRollTorque* antiRollTorque = NULL;
        const PxVehicleCommandState* commandState = NULL;
        const PxVehicleDirectDriveThrottleCommandResponseParams* directDriveThrottleResponseParams = NULL;
        const PxVehicleDirectDriveTransmissionCommandState* directDriveTransmissionState = NULL;
        VEHICLE_FLOAT_ARRAY_DATA(directDrivethrottleResponseState);
        const PxVehicleClutchCommandResponseParams* clutchResponseParams = NULL;
        const PxVehicleClutchParams* clutchParams = NULL;
        const PxVehicleEngineParams* engineParams = NULL;
        const PxVehicleGearboxParams* gearboxParams = NULL;
        const PxVehicleAutoboxParams* autoboxParams = NULL;
        const PxVehicleMultiWheelDriveDifferentialParams* multiWheelDiffParams = NULL;
        const PxVehicleFourWheelDriveDifferentialParams* fourWheelDiffParams = NULL;
        const PxVehicleEngineDriveTransmissionCommandState* engineDriveTransmissionCommandState = NULL;
        const PxVehicleClutchCommandResponseState* clutchResponseState = NULL;
        const PxVehicleEngineDriveThrottleCommandResponseState* throttleResponseState = NULL;
        const PxVehicleEngineState* engineState = NULL;
        const PxVehicleGearboxState* gearboxState = NULL;
        const PxVehicleAutoboxState* autoboxState = NULL;
        const PxVehicleDifferentialState* diffState = NULL;
        const PxVehicleClutchSlipState* clutchSlipState = NULL;
        VEHICLE_ARRAY_DATA(PxVehiclePhysXSuspensionLimitConstraintParams, physxConstraintParams);
        VEHICLE_ARRAY_DATA(PxVehiclePhysXMaterialFrictionParams, physxMaterialFrictionParams);
        const PxVehiclePhysXActor* physxActor = NULL;
        const PxVehiclePhysXRoadGeometryQueryParams* physxRoadGeomQryParams = NULL;
        VEHICLE_ARRAY_DATA(PxVehiclePhysXRoadGeometryQueryState, physxRoadGeomStates);
        VEHICLE_ARRAY_DATA(PxVehiclePhysXConstraintState, physxConstraintStates);
        PxVehiclePvdObjectHandles* omniPvdObjectHandles = NULL;
 
        getDataForPVDComponent(
            axleDesc, 
            rbodyParams, rbodyState,
            suspStateCalcParams,
            brakeResponseParams, steerResponseParams,
            brakeResponseStates, steerResponseStates,
            wheelParams, 
            wheelActuationStates, wheelRigidBody1dStates, wheelLocalPoses,
            roadGeomStates,
            suspParams, suspComplianceParams, suspForceParams,
            suspStates, suspComplianceStates,
            suspForces,
            tireForceParams,
            tireDirectionStates, tireSpeedStates, tireSlipStates, tireStickyStates, tireGripStates, tireCamberStates,
            tireForces,
            antiRollParams, antiRollTorque,
            commandState,
            directDriveThrottleResponseParams, directDriveTransmissionState, directDrivethrottleResponseState,
            clutchResponseParams, clutchParams, engineParams, gearboxParams, autoboxParams, multiWheelDiffParams, fourWheelDiffParams,
            engineDriveTransmissionCommandState,
            clutchResponseState, throttleResponseState, engineState, gearboxState, autoboxState, diffState, clutchSlipState,
            physxConstraintParams, physxMaterialFrictionParams,
            physxActor, physxRoadGeomQryParams, physxRoadGeomStates, physxConstraintStates,
            omniPvdObjectHandles);
 
        if(!omniPvdObjectHandles)
            return true;
 
        if(firstTime)
        {
            PxVehiclePvdRigidBodyRegister(
                rbodyParams, rbodyState,  
                *context.pvdContext.attributeHandles, omniPvdObjectHandles, context.pvdContext.writer);
 
            PxVehiclePvdSuspensionStateCalculationParamsRegister(
                suspStateCalcParams,
                *context.pvdContext.attributeHandles, omniPvdObjectHandles, context.pvdContext.writer);
    
            PxVehiclePvdCommandResponseRegister(
                brakeResponseParams, steerResponseParams, 
                brakeResponseStates, steerResponseStates,
                *context.pvdContext.attributeHandles, omniPvdObjectHandles, context.pvdContext.writer);
            
            PxVehiclePvdWheelAttachmentsRegister(
                *axleDesc,
                wheelParams, wheelActuationStates, wheelRigidBody1dStates, wheelLocalPoses, 
                roadGeomStates, 
                suspParams, suspComplianceParams, suspForceParams, suspStates, suspComplianceStates, 
                suspForces,
                tireForceParams,
                tireDirectionStates, tireSpeedStates, tireSlipStates, tireStickyStates, tireGripStates, tireCamberStates,
                tireForces,
                *context.pvdContext.attributeHandles, omniPvdObjectHandles, context.pvdContext.writer);
 
            PxVehiclePvdAntiRollsRegister(
                antiRollParams, antiRollTorque, 
                *context.pvdContext.attributeHandles, omniPvdObjectHandles, context.pvdContext.writer);
 
            PxVehiclePvdDirectDrivetrainRegister(
                commandState, directDriveTransmissionState, 
                directDriveThrottleResponseParams, 
                directDrivethrottleResponseState, 
                *context.pvdContext.attributeHandles, omniPvdObjectHandles, context.pvdContext.writer);
 
            PxVehiclePvdEngineDrivetrainRegister(
                commandState, engineDriveTransmissionCommandState, 
                clutchResponseParams, clutchParams, engineParams, gearboxParams, autoboxParams, multiWheelDiffParams, fourWheelDiffParams,
                clutchResponseState, throttleResponseState, engineState, gearboxState, autoboxState, diffState, clutchSlipState,
                *context.pvdContext.attributeHandles, omniPvdObjectHandles, context.pvdContext.writer);
 
            PxVehiclePvdPhysXWheelAttachmentRegister(
                *axleDesc, 
                physxConstraintParams, physxMaterialFrictionParams,
                physxActor, physxRoadGeomQryParams,  physxRoadGeomStates, physxConstraintStates,
                *context.pvdContext.attributeHandles, omniPvdObjectHandles, context.pvdContext.writer);
 
            PxVehiclePvdPhysXRigidActorRegister(
                physxActor, 
                *context.pvdContext.attributeHandles, omniPvdObjectHandles, context.pvdContext.writer);
                        
            firstTime = false;
        }
 
        PxVehiclePvdRigidBodyWrite(
            rbodyParams, rbodyState,
            *context.pvdContext.attributeHandles, *omniPvdObjectHandles, context.pvdContext.writer);
 
        PxVehiclePvdSuspensionStateCalculationParamsWrite(
            suspStateCalcParams,
            *context.pvdContext.attributeHandles, *omniPvdObjectHandles, context.pvdContext.writer);
 
        PxVehiclePvdCommandResponseWrite(
            *axleDesc, brakeResponseParams, steerResponseParams, brakeResponseStates, steerResponseStates,
            *context.pvdContext.attributeHandles, *omniPvdObjectHandles, context.pvdContext.writer);
        
        PxVehiclePvdWheelAttachmentsWrite(
            *axleDesc,
            wheelParams, wheelActuationStates, wheelRigidBody1dStates, wheelLocalPoses, 
            roadGeomStates, 
            suspParams, suspComplianceParams, suspForceParams, suspStates, suspComplianceStates, 
            suspForces,
            tireForceParams,
            tireDirectionStates, tireSpeedStates, tireSlipStates, tireStickyStates, tireGripStates, tireCamberStates,
            tireForces,
            *context.pvdContext.attributeHandles, *omniPvdObjectHandles, context.pvdContext.writer);
 
        PxVehiclePvdAntiRollsWrite(
            antiRollParams, antiRollTorque, 
            *context.pvdContext.attributeHandles, *omniPvdObjectHandles, context.pvdContext.writer);
        
        PxVehiclePvdDirectDrivetrainWrite(
            *axleDesc, 
            commandState, directDriveTransmissionState, 
            directDriveThrottleResponseParams, 
            directDrivethrottleResponseState, 
            *context.pvdContext.attributeHandles, *omniPvdObjectHandles, context.pvdContext.writer);
 
        PxVehiclePvdEngineDrivetrainWrite(
            commandState, engineDriveTransmissionCommandState, 
            clutchResponseParams, clutchParams, engineParams, gearboxParams, autoboxParams, multiWheelDiffParams, fourWheelDiffParams,
            clutchResponseState, throttleResponseState, engineState, gearboxState, autoboxState, diffState, clutchSlipState,
            *context.pvdContext.attributeHandles, *omniPvdObjectHandles, context.pvdContext.writer);
 
        PxVehiclePvdPhysXWheelAttachmentWrite(
            *axleDesc, 
            physxConstraintParams, physxMaterialFrictionParams,
            physxActor, physxRoadGeomQryParams, physxRoadGeomStates, physxConstraintStates, 
            *context.pvdContext.attributeHandles, *omniPvdObjectHandles, context.pvdContext.writer);
        
        PxVehiclePvdPhysXRigidActorWrite(
            physxActor, 
            *context.pvdContext.attributeHandles, *omniPvdObjectHandles, context.pvdContext.writer);
 
        return true;
    }
    
private:
 
    bool firstTime;
};
 
#if !PX_DOXYGEN
} // namespace vehicle2
} // namespace physx
#endif