Hoops_Exchange/exchange/exchangesource/Collision/Collision.cpp

/***********************************************************************************************************************
*
* Copyright (c) 2010 - 2025 by Tech Soft 3D, Inc.
* The information contained herein is confidential and proprietary to Tech Soft 3D, Inc., and considered a trade secret
* as defined under civil and criminal statutes. Tech Soft 3D shall pursue its civil and criminal remedies in the event
* of unauthorized use or misappropriation of its trade secrets. Use of this information by anyone other than authorized
* employees of Tech Soft 3D, Inc. is granted only under a written non-disclosure agreement, expressly prescribing the
* scope and manner of such use.
*
***********************************************************************************************************************/
/**
\file Collision.cpp

This program demonstrates how to programmatically use the A3DCollisionCompute function.

This program reads a model file and outputs a text file. Specify these files on the command line (see 'Usage').

A sample file can be find in data directory : data\inventor\collision.iam

Usage:
Collision [INPUT] [OUTPUT] [UUIDs...]
INPUT            The input CAD file. Default is '00242AC8'
OUTPUT           The output result file. Default is 'CURRENT_DIR/Collision.txt'
UUID1 UUID2...   Any number of space separated UUIDs of representation items to be checked to each other. If not set, two relevant UUIDs from the CAD file will be picked

***********************************************************************************************************************/

#ifdef _MSC_VER
#  define SAMPLE_DEFAULT_CAD_FILE _T(SAMPLES_DATA_DIRECTORY"\\inventor\\collision.iam")
#  define SAMPLE_DEFAULT_RESULT_FILE _T("Collision.txt")
#else
#  define SAMPLE_DEFAULT_CAD_FILE SAMPLES_DATA_DIRECTORY"/inventor/collision.iam"
#  define SAMPLE_DEFAULT_RESULT_FILE "Collision.txt"
#endif

#define INITIALIZE_A3D_API
#include <A3DSDKIncludes.h>
#include <hoops_license.h>

#include "../common.hpp"

#include "visitor/VisitorContainer.h"
#include "visitor/VisitorCascadedAttribute.h"
#include "visitor/VisitorTree.h"
#include "visitor/VisitorTransfo.h"
#include "visitor/CascadedAttributeConnector.h"
#include "visitor/TransfoConnector.h"
#include <cassert>
#ifdef _MSC_VER
#	include <codecvt>
#endif
#include <iostream>
#include <locale>
#include <sstream>
#include <fstream>

using string_t = std::basic_string<MY_CHAR>;

// function declared in createssellatedbox.cpp
A3DStatus CreatePolyBrepModelFromABBox(A3DBoundingBoxData const & sABBox, A3DRiPolyBrepModel ** pRiPolyBrepModel);

//######################################################################################################################
struct A3DFlattenVisitor : public A3DTreeVisitor
{
	std::vector<A3DRiRepresentationItem*>	m_vA3DRiBrepModel;
	std::vector<A3DMatrix4x4>				m_vA3DMatrix4x4;
	std::vector<A3DUTF8Char *>				m_vProductIdentifier;

	A3DUTF8Char *							m_pcProductIdentifier = nullptr;

	double									m_dScaleToMm = 1.;

	A3DFlattenVisitor(A3DVisitorContainer* psContainer = NULL) : A3DTreeVisitor(psContainer) { psContainer->SetTraverseInstance(true); }

	~A3DFlattenVisitor()
	{
		for (A3DUTF8Char* p: m_vProductIdentifier)
			free(p);
	}

	A3DStatus visitEnter(const A3DPartConnector& sConnector) override
	{
		A3DStatus iRet = A3DTreeVisitor::visitEnter(sConnector);
		if (iRet == A3D_SUCCESS)
		{
			A3DAsmProductOccurrenceGetIdentifier(sConnector.GetProductOccurrenceFather(), &m_pcProductIdentifier);
		}

		return iRet;
	}

	A3DStatus visitLeave(const A3DPartConnector& sConnector) override
	{
		A3DStatus iRet = A3DTreeVisitor::visitLeave(sConnector);
		if (m_pcProductIdentifier)
		{
			A3DAsmProductOccurrenceGetIdentifier(nullptr, &m_pcProductIdentifier);
		}

		return iRet;
	}

	virtual A3DStatus visitEnter(const A3DRiConnector& sConnector) override
	{
		A3DStatus iRet = A3DTreeVisitor::visitEnter(sConnector);

		const A3DEntity* pRi = sConnector.GetA3DEntity();
		A3DEEntityType eType = kA3DTypeUnknown;
		iRet = A3DEntityGetType(pRi, &eType);
		if ((iRet == A3D_SUCCESS) && (eType == kA3DTypeRiBrepModel || eType == kA3DTypeRiPolyBrepModel))
		{
			A3DVisitorColorMaterials *pA3DCascadedVisitor = static_cast<A3DVisitorColorMaterials*>(m_psContainer->GetVisitorByName("CascadedAttribute"));
			if (pA3DCascadedVisitor)
			{
				ColorMaterialsConnector sColorConnector(nullptr);
				pA3DCascadedVisitor->GetColorMaterialConnector(sColorConnector);

				if (!sColorConnector.IsShow())
					return A3D_SUCCESS;
			}

			m_vA3DRiBrepModel.push_back((A3DRiRepresentationItem*)sConnector.GetA3DEntity());

			A3DVisitorTransfo* psVisitorTransfo = static_cast<A3DVisitorTransfo*>(m_psContainer->GetVisitorByName("Transformation"));
			A3DTransfoConnector* pConnector = psVisitorTransfo->GetTransfoConnector();
			A3DMatrix4x4 sTransfo;
			pConnector->GetGlobalTransfo(sTransfo);
			delete pConnector;

			// Get global transfo
			if (m_dScaleToMm != 1.)
			{
				A3DMatrix4x4 sScale;
				sScale.ResetToIdentity();
				sScale.m_adM[0] = m_dScaleToMm;
				sScale.m_adM[5] = m_dScaleToMm;
				sScale.m_adM[10] = m_dScaleToMm;
				sScale.m_adM[15] = m_dScaleToMm;

				A3DMatrix4x4 sScaledTransfo;
				sScaledTransfo = sTransfo * sScale;
				sScaledTransfo.m_adM[15] = 1.;
				m_vA3DMatrix4x4.push_back(sScaledTransfo);
			}
			else
				m_vA3DMatrix4x4.push_back(sTransfo);

			// Get product occurrence identifier ... assuming that there only on rep item by part
			A3DUTF8Char * pcProductID = nullptr;
			if (m_pcProductIdentifier != nullptr)
			{
				size_t iLen = strlen(m_pcProductIdentifier);
				pcProductID = static_cast<A3DUTF8Char*>(malloc(sizeof(A3DUTF8Char)*(iLen + 1)));
				strcpy(pcProductID, m_pcProductIdentifier);
			}
			m_vProductIdentifier.push_back(pcProductID);

		}
		return iRet;
	}
};

//#####################################################################################################################
A3DStatus stGetFlattenRepItemInMmFromModelFile(	A3DAsmModelFile * pModelFile,
											std::vector<A3DTransfoRepresentationItemData> & apRepItem,
											std::vector<string_t> & asIdentifiers)
{
	A3DAsmModelFileData sMFData;
	A3D_INITIALIZE_DATA(A3DAsmModelFileData, sMFData);
	A3DAsmModelFileGet(pModelFile, &sMFData);
	double dUnitInMm = sMFData.m_dUnit;
	A3DAsmModelFileGet(NULL, &sMFData);

	A3DVisitorContainer sA3DVisitorContainer(CONNECT_TRANSFO);
	sA3DVisitorContainer.SetTraverseInstance(true);

	A3DVisitorColorMaterials *pA3DCascadedVisitor = new A3DVisitorColorMaterials(&sA3DVisitorContainer);
	sA3DVisitorContainer.push(pA3DCascadedVisitor);

	A3DFlattenVisitor *pA3DFlattenVisitor = new A3DFlattenVisitor(&sA3DVisitorContainer);
	sA3DVisitorContainer.push(pA3DFlattenVisitor);
	pA3DFlattenVisitor->m_dScaleToMm = dUnitInMm;

	A3DModelFileConnector sModelFileConnector(pModelFile);
	A3DStatus sStatus = sModelFileConnector.Traverse(&sA3DVisitorContainer);
	if (sStatus != A3D_SUCCESS)
		return A3D_ERROR;

	size_t uiRI, uiNbRepItem = pA3DFlattenVisitor->m_vA3DRiBrepModel.size();
	if (uiNbRepItem == 0)
		return A3D_MODELFILE_INCONSISTENT_EMPTY;

	apRepItem.resize(uiNbRepItem);
	asIdentifiers.resize(uiNbRepItem);
	for (uiRI = 0; uiRI < uiNbRepItem; uiRI++)
	{
		A3DTransfoRepresentationItemData & sTransfRI = apRepItem[uiRI];
		A3D_INITIALIZE_DATA(A3DTransfoRepresentationItemData, sTransfRI);

		sTransfRI.m_pRepItem = pA3DFlattenVisitor->m_vA3DRiBrepModel[uiRI];

		sTransfRI.m_pOptPlacement = new double[16];
		memcpy( const_cast<double*>(sTransfRI.m_pOptPlacement), pA3DFlattenVisitor->m_vA3DMatrix4x4[uiRI].m_adM, 16 * sizeof(double));

		if (A3DUTF8Char* psId = pA3DFlattenVisitor->m_vProductIdentifier[uiRI])
		{
			std::basic_ostringstream<MY_CHAR> stream;
			stream << psId;
			asIdentifiers[uiRI] = stream.str();
		}
	}

	return A3D_SUCCESS;
}

void PrintUsage()
{
#ifdef _MSC_VER
	std::wcout << "Usage:" << std::endl
#else
	std::cout << "Usage:" << std::endl
#endif
		<< "Collision [INPUT] [OUTPUT] [UUIDs...]" << std::endl
		<< "\tINPUT            The input CAD file. Default is '" << SAMPLE_DEFAULT_CAD_FILE << "'" << std::endl
		<< "\tOUTPUT           The output result file. Default is 'CURRENT_DIR/Collision.txt'" << std::endl
		<< "\tUUID1 UUID2...   Any number of space separated UUIDs of representation items to be checked to each other. If not set, two relevant UUIDs from the CAD file will be picked" << std::endl;
}

//######################################################################################################################
#ifdef _MSC_VER
int wmain(A3DInt32 iArgc, A3DUniChar** ppcArgv)
#else
int main(A3DInt32 iArgc, A3DUTF8Char** ppcArgv)
#endif
{
	PrintUsage();

	// INITIALIZE HOOPS EXCHANGE
	A3DSDKHOOPSExchangeLoader sHoopsExchangeLoader(_T(HOOPS_BINARY_DIRECTORY), HOOPS_LICENSE);
	CHECK_RET(sHoopsExchangeLoader.m_eSDKStatus);
	CHECK_RET(A3DDllSetCallbacksMemory(CheckMalloc, CheckFree));
	CHECK_RET(A3DDllSetCallbacksReport(PrintLogMessage, PrintLogWarning, PrintLogError));

	// Parsing arguments
	string_t acInputCADFile = SAMPLE_DEFAULT_CAD_FILE;
	string_t acResultFile = SAMPLE_DEFAULT_RESULT_FILE;
	std::vector<string_t> asInputUUID;

	// Default Input CAD overridden by first argument
	if (iArgc > 1)
	{
		acInputCADFile = ppcArgv[1];
	}

	// Default result file overridden by second argument
	if (iArgc > 2)
	{
		acResultFile = ppcArgv[2];
	}

	// Get all UUIDs
	for (A3DInt32 i = 3; i < iArgc; ++i)
	{
		asInputUUID.emplace_back(ppcArgv[i]);
	}

	//#####################################
	// ### Import input XML file
	//#####################################
	A3DImport sImport(acInputCADFile.c_str());

	A3DStatus    iRet   = A3D_ERROR;
	const size_t uiSize = acInputCADFile.size();
	if (uiSize >= 4)
	{
		const string_t sExtension = acInputCADFile.substr(uiSize - 4, 4);
#ifdef _MSC_VER
		if (sExtension == _T(".xml") || sExtension == _T(".XML") || sExtension == _T(".Xml"))
		{
			std::wstring_convert<std::codecvt_utf8_utf16<wchar_t>> converter;
			const std::string aScrFileNameUTF8 = converter.to_bytes(acInputCADFile);
			iRet = A3DAsmModelFileLoadFromXMLFile(aScrFileNameUTF8.c_str(), &sImport.m_sLoadData, &sHoopsExchangeLoader.m_psModelFile);
		}
#else
		if (sExtension == ".xml" || sExtension == ".XML" || sExtension == ".Xml")
			iRet = A3DAsmModelFileLoadFromXMLFile(acInputCADFile.c_str(), &sImport.m_sLoadData, &sHoopsExchangeLoader.m_psModelFile);
#endif
		else
			iRet = sHoopsExchangeLoader.Import(sImport);
	}
	else
		iRet = sHoopsExchangeLoader.Import(sImport);

	if (iRet != A3D_SUCCESS && iRet != A3D_LOAD_MISSING_COMPONENTS)
		CHECK_RET(iRet);

	//#####################################
	// ### Flatten Representation Items
	//#####################################
	std::vector<A3DTransfoRepresentationItemData> apRepItem;
	std::vector<string_t> asIdentifier;
	iRet = stGetFlattenRepItemInMmFromModelFile(sHoopsExchangeLoader.m_psModelFile, apRepItem, asIdentifier);
	CHECK_RET(iRet);

	//#####################################
	// ### Some Checks
	//#####################################
	if (apRepItem.size() < 2)
	{
		std::cout << "Collision sample needs at least 2 representation items in the model file to process." << std::endl;
		return A3D_ERROR;
	}
	assert(apRepItem.size() == asIdentifier.size());

	// If no UUID given by the user, take the first available
	if (asInputUUID.empty())
	{
		asInputUUID.push_back(asIdentifier.front());
	}

	////#####################################
	//// ### Build collision group
	////#####################################
	std::vector<A3DTransfoRepresentationItemData> asGroup1, asGroup2;
	std::vector<size_t> auiGroupIndex1, auiGroupIndex2;
	for (size_t iIndex = 0; iIndex < apRepItem.size(); iIndex++)
	{
		bool bInSecondGroup = true;
		string_t const & sCurID = asIdentifier[iIndex];
		for (auto const & sInputID : asInputUUID)
		{
			if (sCurID.compare(sInputID) == 0)
			{
				asGroup1.push_back(apRepItem[iIndex]);
				auiGroupIndex1.push_back(iIndex);
				bInSecondGroup = false;
				break;
			}
		}
		if (bInSecondGroup)
		{
			asGroup2.push_back(apRepItem[iIndex]);
			auiGroupIndex2.push_back(iIndex);
		}
	}

	// fill first group structure
	A3DCollisionGroupData sGroup1;
	A3D_INITIALIZE_DATA(A3DCollisionGroupData, sGroup1);
	sGroup1.m_uRepItemSize = (A3DUns32)asGroup1.size();
	if (sGroup1.m_uRepItemSize)
		sGroup1.m_apRepItems = &asGroup1[0];
	else
	{
		printf("Invalid first group\n");
		return -1;
	}

	// fill second group structure
	A3DCollisionGroupData sGroup2;
	A3D_INITIALIZE_DATA(A3DCollisionGroupData, sGroup2);
	sGroup2.m_uRepItemSize = (A3DUns32)asGroup2.size();
	if (sGroup2.m_uRepItemSize)
		sGroup2.m_apRepItems = &asGroup2[0];


	//#####################################
	// ### Specify collision parameters
	//#####################################

	A3DCollisionParameterData sCollisionParameter;
	A3D_INITIALIZE_DATA(A3DCollisionParameterData, sCollisionParameter);
	sCollisionParameter.m_dContactLimit = 0.1;
	sCollisionParameter.m_dSafetyDistance = 1.;
	sCollisionParameter.m_dTessellationTolerance = 0.01;

	//#####################################
	// ### Compute
	//#####################################

	A3DUns32 uCollisionResultsSize = 0;
	A3DCollisionResultData * apCollisionResults = nullptr;
	iRet = A3DCollisionCompute(&sGroup1, sGroup2.m_uRepItemSize?&sGroup2:nullptr,
								&sCollisionParameter,
								&uCollisionResultsSize, &apCollisionResults);
	CHECK_RET(iRet);

	std::basic_ofstream<MY_CHAR> sOutput;
	sOutput.open(acResultFile);
	if (sOutput.is_open())
	{
		for (A3DUns32 uiRes = 0; uiRes < uCollisionResultsSize; uiRes++)
		{
			A3DCollisionResultData const & sCurResult = apCollisionResults[uiRes];

			size_t iGlobalIndex1 = auiGroupIndex1[sCurResult.m_iRepItemIndex1];
			size_t iGlobalIndex2 = (sGroup2.m_uRepItemSize != 0) ? auiGroupIndex2[sCurResult.m_iRepItemIndex2] : auiGroupIndex1[sCurResult.m_iRepItemIndex2];

			sOutput << "Check " << asIdentifier[iGlobalIndex1] << " with " << asIdentifier[iGlobalIndex2] << " : ";

			switch (sCurResult.m_eStatus)
			{
			case A3DCollision_Unknown:		sOutput << "Unknown"; break;
			case A3DCollision_NoCollision:	sOutput << "No Collision"; break;
			case A3DCollision_Clearance:	sOutput << "Clearance"; break;
			case A3DCollision_Contact:		sOutput << "Contact"; break;
			case A3DCollision_Collision:	sOutput << "Collision"; break;
			case A3DCollision_FirstInside:	sOutput << "First is Inside"; break;
			case A3DCollision_SecondInside:	sOutput << "Second is Inside"; break;
			}

			sOutput << std::endl;
		}

		sOutput.close();
	}

	// Free memory
	iRet = A3DCollisionCompute(nullptr, nullptr, nullptr, &uCollisionResultsSize, &apCollisionResults);
	CHECK_RET(iRet);

	for (auto & sTransfoRep : apRepItem)
	{
		delete [] sTransfoRep.m_pOptPlacement;
	}

	// Check memory allocations
	return (int)ListLeaks();
}