Method DrawDebugGeom

Source/URLab/Private/MuJoCo/Utils/MjUtils.cpp:151–279

}

void MjUtils::DrawDebugGeom(UWorld* World, const mjModel* m, const GeomView& geom_view, const FColor& DrawColor, float Multiplier)
{
	if (!World || !m)
		return;

	mjtNum* pos = geom_view.geom_xpos;
	mjtNum* mat = geom_view.geom_xmat;
	mjtNum* size = geom_view.geom_size;

	// Draw if group 3 (collision convention) OR if both contype and conaffinity are non-zero (active collider)
	int group = geom_view._m->geom_group[geom_view.id];
	int contype = geom_view._m->geom_contype[geom_view.id];
	int conaffinity = geom_view._m->geom_conaffinity[geom_view.id];
	bool isCollisionGeom = (group == 3) || (contype != 0 && conaffinity != 0);
	if (!isCollisionGeom)
	{
		return;
	}

	FVector Position = MjToUEPosition(pos);

	mjtNum _quat[4];
	mju_mat2Quat(_quat, mat);
	FQuat Rotation = MjToUERotation(_quat);

	switch (geom_view.geom_type)
	{
		case mjGEOM_BOX:
		{
			FColor Color = DrawColor != FColor::Magenta ? DrawColor : FColor::Cyan;
			FVector Extent(size[0] * Multiplier, size[1] * Multiplier, size[2] * Multiplier);
			DrawDebugBox(World, Position, Extent, Rotation, Color, false, -1, 0, 0.15f);
			break;
		}
		case mjGEOM_SPHERE:
		{
			FColor Color = DrawColor != FColor::Magenta ? DrawColor : FColor::Green;
			float Radius = size[0] * Multiplier;
			DrawDebugSphere(World, Position, Radius, 16, Color, false, -1, 0, 0.15f);
			break;
		}
		case mjGEOM_CAPSULE:
		{
			FColor Color = DrawColor != FColor::Magenta ? DrawColor : FColor::Yellow;
			float Radius = size[0] * Multiplier;
			float HalfHeight = size[1] * Multiplier;
			DrawDebugCapsule(World, Position, HalfHeight + Radius, Radius, Rotation, Color, false, -1, 0, 0.15f);
			break;
		}
		case mjGEOM_CYLINDER:
		{
			FColor Color = DrawColor != FColor::Magenta ? DrawColor : FColor::Orange;
			float Radius = size[0] * Multiplier;
			float HalfHeight = size[1] * Multiplier;
			FVector UpVector = Rotation.GetAxisZ();
			FVector Start = Position - UpVector * HalfHeight;
			FVector End = Position + UpVector * HalfHeight;
			DrawDebugCylinder(World, Start, End, Radius, 16, Color, false, -1, 0, 0.15f);