<commit_before>
#include "input.h"
#include <QDebug>

namespace NeovimQt {

InputConv Input;

InputConv::InputConv() {
	// see :h key-notation

	// special keys i.e. no textual representation
	specialKeys.insert(Qt::Key_Up, "Up");
	specialKeys.insert(Qt::Key_Down, "Down");
	specialKeys.insert(Qt::Key_Left, "Left");
	specialKeys.insert(Qt::Key_Right, "Right");

	specialKeys.insert(Qt::Key_F1, "F1");
	specialKeys.insert(Qt::Key_F2, "F2");
	specialKeys.insert(Qt::Key_F3, "F3");
	specialKeys.insert(Qt::Key_F4, "F4");
	specialKeys.insert(Qt::Key_F5, "F5");
	specialKeys.insert(Qt::Key_F6, "F6");
	specialKeys.insert(Qt::Key_F7, "F7");
	specialKeys.insert(Qt::Key_F8, "F8");
	specialKeys.insert(Qt::Key_F9, "F9");
	specialKeys.insert(Qt::Key_F10, "F10");
	specialKeys.insert(Qt::Key_F11, "F11");
	specialKeys.insert(Qt::Key_F12, "F12");

	specialKeys.insert(Qt::Key_Backspace, "BS");
	specialKeys.insert(Qt::Key_Delete, "Del");
	specialKeys.insert(Qt::Key_Insert, "Insert");
	specialKeys.insert(Qt::Key_Home, "Home");
	specialKeys.insert(Qt::Key_End, "End");
	specialKeys.insert(Qt::Key_PageUp, "PageUp");
	specialKeys.insert(Qt::Key_PageDown, "PageDown");

	specialKeys.insert(Qt::Key_Return, "Enter");
	specialKeys.insert(Qt::Key_Enter, "Enter");
	specialKeys.insert(Qt::Key_Tab, "Tab");
	specialKeys.insert(Qt::Key_Backtab, "Tab");
	specialKeys.insert(Qt::Key_Escape, "Esc");

	specialKeys.insert(Qt::Key_Backslash, "Bslash");
	specialKeys.insert(Qt::Key_Space, "Space");
}

/**
 * Return keyboard modifier prefix
 *
 * e.g. C-, A- or C-S-A-
 *
 * WIN32: Ctrl+Alt are never passed together, since we can't distinguish
 * between Ctrl+Alt and AltGr (see Vim/os_win32.c).
 */
QString InputConv::modPrefix(Qt::KeyboardModifiers mod)
{
	QString modprefix;
#if defined(Q_OS_MAC) || defined(Q_OS_UNIX)
	if ( mod & CmdModifier ) {
		modprefix += "D-"; // like MacVim does
	}
#endif
	if ( mod & ControlModifier
#ifdef Q_OS_WIN32
		&& !(mod & AltModifier)
#endif
	   ) {
		modprefix += "C-";
	}
	if ( mod & ShiftModifier ) {
		modprefix += "S-";
	}
	if ( mod & AltModifier
#ifdef Q_OS_WIN32
		&& !(mod & ControlModifier)
#endif
	   ) {
		modprefix += "A-";
	}
	return modprefix;
}

/**
 * Convert mouse event information into Neovim key notation
 *
 * @type is one of the Qt mouse event types
 * @pos is in Neovim Coordinates
 * @clickCount is the number of consecutive mouse clicks
 *   1 for a single click, 2 for a double click, up to 4.
 *   This value is only used for LeftMouse events.
 *
 * see QMouseEvent
 *
 * If the event is not valid for Neovim, returns an empty string
 */
QString InputConv::convertMouse(Qt::MouseButton bt, QEvent::Type type, Qt::KeyboardModifiers mod, QPoint pos, short clickCount)
{
	QString buttonName;
	switch(bt) {
	case Qt::LeftButton:
		// In practice Neovim only supports the clickcount for Left
		// mouse presses, even if our shell can support other buttons
		if (clickCount > 1 && clickCount <= 4) {
			buttonName = QString("%1-Left").arg(clickCount);
		} else {
			buttonName += "Left";
		}
		break;
	case Qt::RightButton:
		buttonName += "Right";
		break;
	case Qt::MidButton:
		buttonName += "Middle";
		break;
	case Qt::NoButton:
		break;
	default:
		return "";
	}

	QString evType;
	switch(type) {
	case QEvent::MouseButtonDblClick:
		// Treat this as a regular MouseButtonPress. Repeated
		// clicks are handled above.
	case QEvent::MouseButtonPress:
		evType += "Mouse";
		break;
	case QEvent::MouseButtonRelease:
		evType += "Release";
		break;
	case QEvent::MouseMove:
		evType += "Drag";
		break;
	default:
		return "";
	}

	QString inp = QString("<%1%2%3><%4,%5>").arg(modPrefix(mod)).arg(buttonName).arg(evType).arg(pos.x()).arg(pos.y());
	return inp;
}

/**
 * Convert Qt key input into Neovim key-notation
 *
 * see QKeyEvent
 */
QString InputConv::convertKey(const QString& text, int k, Qt::KeyboardModifiers mod)
{
	if (specialKeys.contains(k)) {
		return QString("<%1%2>").arg(modPrefix(mod)).arg(specialKeys.value(k));
	}

	QChar c;
	// Escape < and backslash
	if (text == "<") {
		return QString("<lt>");
	} else if (text == "\\") {
		return QString("<%1%2>").arg(modPrefix(mod)).arg("Bslash");
	} else if (text.isEmpty()) {
		// on macs, text is empty for ctrl+key and cmd+key combos (with or without alt)
		if (mod & ControlModifier || mod & CmdModifier) {
			// ignore ctrl, alt and cmd key combos by themselves
			QList<Qt::Key> keys = { Key_Control, Key_Alt, Key_Cmd };
			if (keys.contains((Qt::Key)k)) {
				return QString();
			} else {
				// key code will be the value of the char (hopefully)
				c = QChar(k);
			}
		} else {
			// This is a special key we can't handle
			return QString();
		}
	} else {
		// Key event compression is disabled, text has one char
		c = text.at(0);
	}

	// Remove SHIFT
	if (c.unicode() < 0x100 && !c.isLetterOrNumber() && c.isPrint()) {
		mod &= ~ShiftModifier;
	}

	// Remove CTRL empty characters at the start of the ASCII range
	if (c.unicode() < 0x20) {
		mod &= ~ControlModifier;
	}

	// Format with prefix if necessary
	QString prefix = modPrefix(mod);
	if (!prefix.isEmpty()) {
		return QString("<%1%2>").arg(prefix).arg(c);
	}

	return QString(c);
}

} // Namespace
<commit_msg>recognize keypad<commit_after>
#include "input.h"
#include <QDebug>

namespace NeovimQt {

InputConv Input;

InputConv::InputConv() {
	// see :h key-notation

	// special keys i.e. no textual representation
	specialKeys.insert(Qt::Key_Up, "Up");
	specialKeys.insert(Qt::Key_Down, "Down");
	specialKeys.insert(Qt::Key_Left, "Left");
	specialKeys.insert(Qt::Key_Right, "Right");

	specialKeys.insert(Qt::Key_F1, "F1");
	specialKeys.insert(Qt::Key_F2, "F2");
	specialKeys.insert(Qt::Key_F3, "F3");
	specialKeys.insert(Qt::Key_F4, "F4");
	specialKeys.insert(Qt::Key_F5, "F5");
	specialKeys.insert(Qt::Key_F6, "F6");
	specialKeys.insert(Qt::Key_F7, "F7");
	specialKeys.insert(Qt::Key_F8, "F8");
	specialKeys.insert(Qt::Key_F9, "F9");
	specialKeys.insert(Qt::Key_F10, "F10");
	specialKeys.insert(Qt::Key_F11, "F11");
	specialKeys.insert(Qt::Key_F12, "F12");

	specialKeys.insert(Qt::Key_Backspace, "BS");
	specialKeys.insert(Qt::Key_Delete, "Del");
	specialKeys.insert(Qt::Key_Insert, "Insert");
	specialKeys.insert(Qt::Key_Home, "Home");
	specialKeys.insert(Qt::Key_End, "End");
	specialKeys.insert(Qt::Key_PageUp, "PageUp");
	specialKeys.insert(Qt::Key_PageDown, "PageDown");

	specialKeys.insert(Qt::Key_Return, "Enter");
	specialKeys.insert(Qt::Key_Enter, "Enter");
	specialKeys.insert(Qt::Key_Tab, "Tab");
	specialKeys.insert(Qt::Key_Backtab, "Tab");
	specialKeys.insert(Qt::Key_Escape, "Esc");

	specialKeys.insert(Qt::Key_Backslash, "Bslash");
	specialKeys.insert(Qt::Key_Space, "Space");
}

/**
 * Return keyboard modifier prefix
 *
 * e.g. C-, A- or C-S-A-
 *
 * WIN32: Ctrl+Alt are never passed together, since we can't distinguish
 * between Ctrl+Alt and AltGr (see Vim/os_win32.c).
 */
QString InputConv::modPrefix(Qt::KeyboardModifiers mod)
{
	QString modprefix;
#if defined(Q_OS_MAC) || defined(Q_OS_UNIX)
	if ( mod & CmdModifier ) {
		modprefix += "D-"; // like MacVim does
	}
#endif
	if ( mod & ControlModifier
#ifdef Q_OS_WIN32
		&& !(mod & AltModifier)
#endif
	   ) {
		modprefix += "C-";
	}
	if ( mod & ShiftModifier ) {
		modprefix += "S-";
	}
	if ( mod & AltModifier
#ifdef Q_OS_WIN32
		&& !(mod & ControlModifier)
#endif
	   ) {
		modprefix += "A-";
	}
    if ( mod & Qt::KeypadModifier ) {
        modprefix += "k";
    }

	return modprefix;
}

/**
 * Convert mouse event information into Neovim key notation
 *
 * @type is one of the Qt mouse event types
 * @pos is in Neovim Coordinates
 * @clickCount is the number of consecutive mouse clicks
 *   1 for a single click, 2 for a double click, up to 4.
 *   This value is only used for LeftMouse events.
 *
 * see QMouseEvent
 *
 * If the event is not valid for Neovim, returns an empty string
 */
QString InputConv::convertMouse(Qt::MouseButton bt, QEvent::Type type, Qt::KeyboardModifiers mod, QPoint pos, short clickCount)
{
	QString buttonName;
	switch(bt) {
	case Qt::LeftButton:
		// In practice Neovim only supports the clickcount for Left
		// mouse presses, even if our shell can support other buttons
		if (clickCount > 1 && clickCount <= 4) {
			buttonName = QString("%1-Left").arg(clickCount);
		} else {
			buttonName += "Left";
		}
		break;
	case Qt::RightButton:
		buttonName += "Right";
		break;
	case Qt::MidButton:
		buttonName += "Middle";
		break;
	case Qt::NoButton:
		break;
	default:
		return "";
	}

	QString evType;
	switch(type) {
	case QEvent::MouseButtonDblClick:
		// Treat this as a regular MouseButtonPress. Repeated
		// clicks are handled above.
	case QEvent::MouseButtonPress:
		evType += "Mouse";
		break;
	case QEvent::MouseButtonRelease:
		evType += "Release";
		break;
	case QEvent::MouseMove:
		evType += "Drag";
		break;
	default:
		return "";
	}

	QString inp = QString("<%1%2%3><%4,%5>").arg(modPrefix(mod)).arg(buttonName).arg(evType).arg(pos.x()).arg(pos.y());
	return inp;
}

/**
 * Convert Qt key input into Neovim key-notation
 *
 * see QKeyEvent
 */
QString InputConv::convertKey(const QString& text, int k, Qt::KeyboardModifiers mod)
{
	if (specialKeys.contains(k)) {
		return QString("<%1%2>").arg(modPrefix(mod)).arg(specialKeys.value(k));
	}

	QChar c;
	// Escape < and backslash
	if (text == "<") {
		return QString("<lt>");
	} else if (text == "\\") {
		return QString("<%1%2>").arg(modPrefix(mod)).arg("Bslash");
	} else if (text.isEmpty()) {
		// on macs, text is empty for ctrl+key and cmd+key combos (with or without alt)
		if (mod & ControlModifier || mod & CmdModifier) {
			// ignore ctrl, alt and cmd key combos by themselves
			QList<Qt::Key> keys = { Key_Control, Key_Alt, Key_Cmd };
			if (keys.contains((Qt::Key)k)) {
				return QString();
			} else {
				// key code will be the value of the char (hopefully)
				c = QChar(k);
			}
		} else {
			// This is a special key we can't handle
			return QString();
		}
	} else {
		// Key event compression is disabled, text has one char
		c = text.at(0);
	}

	// Remove SHIFT
	if (c.unicode() < 0x100 && !c.isLetterOrNumber() && c.isPrint()) {
		mod &= ~ShiftModifier;
	}

	// Remove CTRL empty characters at the start of the ASCII range
	if (c.unicode() < 0x20) {
		mod &= ~ControlModifier;
	}

	// Format with prefix if necessary
	QString prefix = modPrefix(mod);
	if (!prefix.isEmpty()) {
		return QString("<%1%2>").arg(prefix).arg(c);
	}

	return QString(c);
}

} // Namespace
<|endoftext|>


<commit_before>/*
Copyright (c) 2013 Daniele Bartolini, Michele Rossi
Copyright (c) 2012 Daniele Bartolini, Simone Boscaratto

Permission is hereby granted, free of charge, to any person
obtaining a copy of this software and associated documentation
files (the "Software"), to deal in the Software without
restriction, including without limitation the rights to use,
copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the
Software is furnished to do so, subject to the following
conditions:

The above copyright notice and this permission notice shall be
included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
OTHER DEALINGS IN THE SOFTWARE.
*/

#include "Unit.h"
#include "World.h"
#include "Allocator.h"
#include "Log.h"
#include "UnitResource.h"
#include "SceneGraphManager.h"
#include "PhysicsGraphManager.h"

namespace crown
{

typedef Id CameraId;
typedef Id SpriteId;

Unit::Unit(World& w, SceneGraph& sg, PhysicsGraph& pg, const UnitResource* ur, const Matrix4x4& pose)
	: m_world(w)
	, m_scene_graph(sg)
	, m_physics_graph(pg)
	, m_resource(ur)
	, m_num_cameras(0)
	, m_num_meshes(0)
	, m_num_sprites(0)
{
	// Log debug info
	Log::d("Creating unit...");
	Log::d("Num renderables       = %d", ur->num_renderables());
	Log::d("Num cameras           = %d", ur->num_cameras());
	Log::d("Num actors            = %d", ur->num_actors());
	Log::d("Num scene graph nodes = %d", ur->num_scene_graph_nodes());

	create(pose);
}

//-----------------------------------------------------------------------------
void Unit::set_id(const UnitId id)
{
	m_id = id;
}

//-----------------------------------------------------------------------------
UnitId Unit::id()
{
	return m_id;
}

//-----------------------------------------------------------------------------
void Unit::create(const Matrix4x4& pose)
{
	// Create the root node
	int32_t root_node = m_scene_graph.create_node(-1, pose);
	int32_t p_root_node = m_physics_graph.create_node(-1, pose);

	// Create renderables
	for (uint32_t i = 0; i < m_resource->num_renderables(); i++)
	{
		int32_t node = m_scene_graph.create_node(root_node, Vector3::ZERO, Quaternion::IDENTITY);

		UnitRenderable renderable = m_resource->get_renderable(i);

		switch (renderable.type)
		{
			case UnitRenderable::MESH:
			{
				MeshId mesh = m_world.create_mesh(renderable.resource, m_scene_graph, node);
				add_mesh(renderable.name, mesh);
				break;
			}
			case UnitRenderable::SPRITE:
			{
				SpriteId sprite = m_world.create_sprite(renderable.resource, m_scene_graph, node);
				add_sprite(renderable.name, sprite);
				break;
			}
			default:
			{
				CE_FATAL("Oops, bad renderable type");
				break;
			}
		}
	}

	// Create cameras
	for (uint32_t i = 0; i < m_resource->num_cameras(); i++)
	{
		const int32_t cam_node = m_scene_graph.create_node(root_node, Vector3::ZERO, Quaternion::IDENTITY);

		UnitCamera camera = m_resource->get_camera(i);
		CameraId cam = m_world.create_camera(m_scene_graph, cam_node);

		add_camera(camera.name, cam);
	}

	// Create actors
	for (uint32_t i = 0; i < m_resource->num_actors(); i++)
	{
		const int32_t actor_node = m_physics_graph.create_node(p_root_node, Vector3::ZERO, Quaternion::IDENTITY);
		UnitActor actor = m_resource->get_actor(i);
		ActorId actor_id = m_world.create_actor(m_physics_graph, actor_node, ActorType::DYNAMIC);

		add_actor(actor.name, actor_id);
	}
}

//-----------------------------------------------------------------------------
void Unit::destroy()
{
	// Destroy cameras
	for (uint32_t i = 0; i < m_num_cameras; i++)
	{
		m_world.destroy_camera(m_cameras[i].component);
	}

	// Destroy meshes
	for (uint32_t i = 0; i < m_num_meshes; i++)
	{
		m_world.destroy_mesh(m_meshes[i].component);
	}

	// Destroy sprites
	for (uint32_t i = 0; i < m_num_sprites; i++)
	{
		m_world.destroy_sprite(m_sprites[i].component);
	}
}

//-----------------------------------------------------------------------------
Vector3 Unit::local_position(int32_t node) const
{
	return m_scene_graph.local_position(node);
}

//-----------------------------------------------------------------------------
Quaternion Unit::local_rotation(int32_t node) const
{
	return m_scene_graph.local_rotation(node);
}

//-----------------------------------------------------------------------------
Matrix4x4 Unit::local_pose(int32_t node) const
{
	return m_scene_graph.local_pose(node);
}

//-----------------------------------------------------------------------------
Vector3 Unit::world_position(int32_t node) const
{
	return m_scene_graph.world_position(node);
}

//-----------------------------------------------------------------------------
Quaternion Unit::world_rotation(int32_t node) const
{
	return m_scene_graph.world_rotation(node);
}

//-----------------------------------------------------------------------------
Matrix4x4 Unit::world_pose(int32_t node) const
{
	return m_scene_graph.world_pose(node);
}

//-----------------------------------------------------------------------------
void Unit::set_local_position(int32_t node, const Vector3& pos)
{
	m_scene_graph.set_local_position(node, pos);
}

//-----------------------------------------------------------------------------
void Unit::set_local_rotation(int32_t node, const Quaternion& rot)
{
	m_scene_graph.set_local_rotation(node, rot);
}

//-----------------------------------------------------------------------------
void Unit::set_local_pose(int32_t node, const Matrix4x4& pose)
{
	m_scene_graph.set_local_pose(node, pose);
}

//-----------------------------------------------------------------------------
void Unit::link_node(int32_t child, int32_t parent)
{
	m_scene_graph.link(child, parent);
}

//-----------------------------------------------------------------------------
void Unit::unlink_node(int32_t child)
{
	m_scene_graph.unlink(child);
}

//-----------------------------------------------------------------------------
void Unit::add_component(StringId32 name, Id component, uint32_t& size, Component* array)
{
	Component comp;
	comp.name = name;
	comp.component = component;

	array[size] = comp;
	size++;
}

//-----------------------------------------------------------------------------
Id Unit::find_component(const char* name, uint32_t size, Component* array)
{
	uint32_t name_hash = hash::murmur2_32(name, string::strlen(name), 0);

	Id comp;
	comp.id = INVALID_ID;

	for (uint32_t i = 0; i < size; i++)
	{
		if (name_hash == array[i].name)
		{
			comp = array[i].component;
		}
	}

	return comp;
}

//-----------------------------------------------------------------------------
Id Unit::find_component(uint32_t index, uint32_t size, Component* array)
{
	Id comp;
	comp.id = INVALID_ID;

	if (index < size)
	{
		comp = array[index].component;
	}

	return comp;
}

//-----------------------------------------------------------------------------
void Unit::add_camera(StringId32 name, CameraId camera)
{
	CE_ASSERT(m_num_cameras < MAX_CAMERA_COMPONENTS, "Max camera number reached");

	add_component(name, camera, m_num_cameras, m_cameras);
}

//-----------------------------------------------------------------------------
void Unit::add_mesh(StringId32 name, MeshId mesh)
{
	CE_ASSERT(m_num_meshes < MAX_MESH_COMPONENTS, "Max mesh number reached");

	add_component(name, mesh, m_num_meshes, m_meshes);
}

//-----------------------------------------------------------------------------
void Unit::add_sprite(StringId32 name, SpriteId sprite)
{
	CE_ASSERT(m_num_sprites < MAX_SPRITE_COMPONENTS, "Max sprite number reached");

	add_component(name, sprite, m_num_sprites, m_sprites);
}

//-----------------------------------------------------------------------------
void Unit::add_actor(StringId32 name, ActorId actor)
{
	CE_ASSERT(m_num_actors < MAX_ACTOR_COMPONENTS, "Max actor number reached");

	add_component(name, actor, m_num_actors, m_actors);
}

//-----------------------------------------------------------------------------
Camera* Unit::camera(const char* name)
{
	CameraId cam = find_component(name, m_num_cameras, m_cameras);

	CE_ASSERT(cam.id != INVALID_ID, "Unit does not have camera with name '%s'", name);

	return m_world.lookup_camera(cam);
}

//-----------------------------------------------------------------------------
Camera* Unit::camera(uint32_t i)
{
	CameraId cam = find_component(i, m_num_cameras, m_cameras);

	CE_ASSERT(cam.id != INVALID_ID, "Unit does not have camera with index '%d'", i);

	return m_world.lookup_camera(cam);
}

//-----------------------------------------------------------------------------
Mesh* Unit::mesh(const char* name)
{
	MeshId mesh = find_component(name, m_num_meshes, m_meshes);

	CE_ASSERT(mesh.id != INVALID_ID, "Unit does not have mesh with name '%s'", name);

	return m_world.lookup_mesh(mesh);
}

//-----------------------------------------------------------------------------
Mesh* Unit::mesh(uint32_t i)
{
	MeshId mesh = find_component(i, m_num_meshes, m_meshes);

	CE_ASSERT(mesh.id != INVALID_ID, "Unit does not have mesh with index '%d'", i);

	return m_world.lookup_mesh(mesh);
}

//-----------------------------------------------------------------------------
Sprite*	Unit::sprite(const char* name)
{
	SpriteId sprite = find_component(name, m_num_sprites, m_sprites);

	CE_ASSERT(sprite.id != INVALID_ID, "Unit does not have sprite with name '%s'", name);

	return m_world.lookup_sprite(sprite);
}

//-----------------------------------------------------------------------------
Sprite*	Unit::sprite(uint32_t i)
{
	SpriteId sprite = find_component(i, m_num_sprites, m_sprites);

	CE_ASSERT(sprite.id != INVALID_ID, "Unit does not have sprite with index '%d'", i);

	return m_world.lookup_sprite(sprite);
}

//-----------------------------------------------------------------------------
Actor* Unit::actor(const char* name)
{
	ActorId actor = find_component(name, m_num_actors, m_actors);

	CE_ASSERT(actor.id != INVALID_ID, "Unit does not have actor with name '%s'", name);

	return m_world.lookup_actor(actor);
}

//-----------------------------------------------------------------------------
Actor* Unit::actor(uint32_t i)
{
	ActorId actor = find_component(i, m_num_actors, m_actors);

	CE_ASSERT(actor.id != INVALID_ID, "Unit does not have actor with name '%d'", i);

	return m_world.lookup_actor(actor);
}

} // namespace crown
<commit_msg>Use UnitResource data to set poses of objects<commit_after>/*
Copyright (c) 2013 Daniele Bartolini, Michele Rossi
Copyright (c) 2012 Daniele Bartolini, Simone Boscaratto

Permission is hereby granted, free of charge, to any person
obtaining a copy of this software and associated documentation
files (the "Software"), to deal in the Software without
restriction, including without limitation the rights to use,
copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the
Software is furnished to do so, subject to the following
conditions:

The above copyright notice and this permission notice shall be
included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
OTHER DEALINGS IN THE SOFTWARE.
*/

#include "Unit.h"
#include "World.h"
#include "Allocator.h"
#include "Log.h"
#include "UnitResource.h"
#include "SceneGraphManager.h"
#include "PhysicsGraphManager.h"

namespace crown
{

typedef Id CameraId;
typedef Id SpriteId;

Unit::Unit(World& w, SceneGraph& sg, PhysicsGraph& pg, const UnitResource* ur, const Matrix4x4& pose)
	: m_world(w)
	, m_scene_graph(sg)
	, m_physics_graph(pg)
	, m_resource(ur)
	, m_num_cameras(0)
	, m_num_meshes(0)
	, m_num_sprites(0)
{
	// Log debug info
	Log::d("Creating unit...");
	Log::d("Num renderables       = %d", ur->num_renderables());
	Log::d("Num cameras           = %d", ur->num_cameras());
	Log::d("Num actors            = %d", ur->num_actors());
	Log::d("Num scene graph nodes = %d", ur->num_scene_graph_nodes());

	create(pose);
}

//-----------------------------------------------------------------------------
void Unit::set_id(const UnitId id)
{
	m_id = id;
}

//-----------------------------------------------------------------------------
UnitId Unit::id()
{
	return m_id;
}

//-----------------------------------------------------------------------------
void Unit::create(const Matrix4x4& pose)
{
	// Create the scene graph
	m_scene_graph.create(m_resource->num_scene_graph_nodes(), m_resource->scene_graph_names(),
							m_resource->scene_graph_poses(), m_resource->scene_graph_parents());

	int32_t p_root_node = m_physics_graph.create_node(-1, pose);

	// Create renderables
	for (uint32_t i = 0; i < m_resource->num_renderables(); i++)
	{
		UnitRenderable renderable = m_resource->get_renderable(i);

		switch (renderable.type)
		{
			case UnitRenderable::MESH:
			{
				MeshId mesh = m_world.create_mesh(renderable.resource, m_scene_graph, renderable.node);
				add_mesh(renderable.name, mesh);
				break;
			}
			case UnitRenderable::SPRITE:
			{
				SpriteId sprite = m_world.create_sprite(renderable.resource, m_scene_graph, renderable.node);
				add_sprite(renderable.name, sprite);
				break;
			}
			default:
			{
				CE_FATAL("Oops, bad renderable type");
				break;
			}
		}
	}

	// Create cameras
	for (uint32_t i = 0; i < m_resource->num_cameras(); i++)
	{
		UnitCamera camera = m_resource->get_camera(i);
		CameraId cam = m_world.create_camera(m_scene_graph, camera.node);

		add_camera(camera.name, cam);
	}

	// Create actors
	for (uint32_t i = 0; i < m_resource->num_actors(); i++)
	{
		const int32_t actor_node = m_physics_graph.create_node(p_root_node, Vector3::ZERO, Quaternion::IDENTITY);
		UnitActor actor = m_resource->get_actor(i);
		ActorId actor_id = m_world.create_actor(m_physics_graph, actor_node, ActorType::DYNAMIC);

		add_actor(actor.name, actor_id);
	}
}

//-----------------------------------------------------------------------------
void Unit::destroy()
{
	// Destroy cameras
	for (uint32_t i = 0; i < m_num_cameras; i++)
	{
		m_world.destroy_camera(m_cameras[i].component);
	}

	// Destroy meshes
	for (uint32_t i = 0; i < m_num_meshes; i++)
	{
		m_world.destroy_mesh(m_meshes[i].component);
	}

	// Destroy sprites
	for (uint32_t i = 0; i < m_num_sprites; i++)
	{
		m_world.destroy_sprite(m_sprites[i].component);
	}
}

//-----------------------------------------------------------------------------
Vector3 Unit::local_position(int32_t node) const
{
	return m_scene_graph.local_position(node);
}

//-----------------------------------------------------------------------------
Quaternion Unit::local_rotation(int32_t node) const
{
	return m_scene_graph.local_rotation(node);
}

//-----------------------------------------------------------------------------
Matrix4x4 Unit::local_pose(int32_t node) const
{
	return m_scene_graph.local_pose(node);
}

//-----------------------------------------------------------------------------
Vector3 Unit::world_position(int32_t node) const
{
	return m_scene_graph.world_position(node);
}

//-----------------------------------------------------------------------------
Quaternion Unit::world_rotation(int32_t node) const
{
	return m_scene_graph.world_rotation(node);
}

//-----------------------------------------------------------------------------
Matrix4x4 Unit::world_pose(int32_t node) const
{
	return m_scene_graph.world_pose(node);
}

//-----------------------------------------------------------------------------
void Unit::set_local_position(int32_t node, const Vector3& pos)
{
	m_scene_graph.set_local_position(node, pos);
}

//-----------------------------------------------------------------------------
void Unit::set_local_rotation(int32_t node, const Quaternion& rot)
{
	m_scene_graph.set_local_rotation(node, rot);
}

//-----------------------------------------------------------------------------
void Unit::set_local_pose(int32_t node, const Matrix4x4& pose)
{
	m_scene_graph.set_local_pose(node, pose);
}

//-----------------------------------------------------------------------------
void Unit::link_node(int32_t child, int32_t parent)
{
	m_scene_graph.link(child, parent);
}

//-----------------------------------------------------------------------------
void Unit::unlink_node(int32_t child)
{
	m_scene_graph.unlink(child);
}

//-----------------------------------------------------------------------------
void Unit::add_component(StringId32 name, Id component, uint32_t& size, Component* array)
{
	Component comp;
	comp.name = name;
	comp.component = component;

	array[size] = comp;
	size++;
}

//-----------------------------------------------------------------------------
Id Unit::find_component(const char* name, uint32_t size, Component* array)
{
	uint32_t name_hash = hash::murmur2_32(name, string::strlen(name), 0);

	Id comp;
	comp.id = INVALID_ID;

	for (uint32_t i = 0; i < size; i++)
	{
		if (name_hash == array[i].name)
		{
			comp = array[i].component;
		}
	}

	return comp;
}

//-----------------------------------------------------------------------------
Id Unit::find_component(uint32_t index, uint32_t size, Component* array)
{
	Id comp;
	comp.id = INVALID_ID;

	if (index < size)
	{
		comp = array[index].component;
	}

	return comp;
}

//-----------------------------------------------------------------------------
void Unit::add_camera(StringId32 name, CameraId camera)
{
	CE_ASSERT(m_num_cameras < MAX_CAMERA_COMPONENTS, "Max camera number reached");

	add_component(name, camera, m_num_cameras, m_cameras);
}

//-----------------------------------------------------------------------------
void Unit::add_mesh(StringId32 name, MeshId mesh)
{
	CE_ASSERT(m_num_meshes < MAX_MESH_COMPONENTS, "Max mesh number reached");

	add_component(name, mesh, m_num_meshes, m_meshes);
}

//-----------------------------------------------------------------------------
void Unit::add_sprite(StringId32 name, SpriteId sprite)
{
	CE_ASSERT(m_num_sprites < MAX_SPRITE_COMPONENTS, "Max sprite number reached");

	add_component(name, sprite, m_num_sprites, m_sprites);
}

//-----------------------------------------------------------------------------
void Unit::add_actor(StringId32 name, ActorId actor)
{
	CE_ASSERT(m_num_actors < MAX_ACTOR_COMPONENTS, "Max actor number reached");

	add_component(name, actor, m_num_actors, m_actors);
}

//-----------------------------------------------------------------------------
Camera* Unit::camera(const char* name)
{
	CameraId cam = find_component(name, m_num_cameras, m_cameras);

	CE_ASSERT(cam.id != INVALID_ID, "Unit does not have camera with name '%s'", name);

	return m_world.lookup_camera(cam);
}

//-----------------------------------------------------------------------------
Camera* Unit::camera(uint32_t i)
{
	CameraId cam = find_component(i, m_num_cameras, m_cameras);

	CE_ASSERT(cam.id != INVALID_ID, "Unit does not have camera with index '%d'", i);

	return m_world.lookup_camera(cam);
}

//-----------------------------------------------------------------------------
Mesh* Unit::mesh(const char* name)
{
	MeshId mesh = find_component(name, m_num_meshes, m_meshes);

	CE_ASSERT(mesh.id != INVALID_ID, "Unit does not have mesh with name '%s'", name);

	return m_world.lookup_mesh(mesh);
}

//-----------------------------------------------------------------------------
Mesh* Unit::mesh(uint32_t i)
{
	MeshId mesh = find_component(i, m_num_meshes, m_meshes);

	CE_ASSERT(mesh.id != INVALID_ID, "Unit does not have mesh with index '%d'", i);

	return m_world.lookup_mesh(mesh);
}

//-----------------------------------------------------------------------------
Sprite*	Unit::sprite(const char* name)
{
	SpriteId sprite = find_component(name, m_num_sprites, m_sprites);

	CE_ASSERT(sprite.id != INVALID_ID, "Unit does not have sprite with name '%s'", name);

	return m_world.lookup_sprite(sprite);
}

//-----------------------------------------------------------------------------
Sprite*	Unit::sprite(uint32_t i)
{
	SpriteId sprite = find_component(i, m_num_sprites, m_sprites);

	CE_ASSERT(sprite.id != INVALID_ID, "Unit does not have sprite with index '%d'", i);

	return m_world.lookup_sprite(sprite);
}

//-----------------------------------------------------------------------------
Actor* Unit::actor(const char* name)
{
	ActorId actor = find_component(name, m_num_actors, m_actors);

	CE_ASSERT(actor.id != INVALID_ID, "Unit does not have actor with name '%s'", name);

	return m_world.lookup_actor(actor);
}

//-----------------------------------------------------------------------------
Actor* Unit::actor(uint32_t i)
{
	ActorId actor = find_component(i, m_num_actors, m_actors);

	CE_ASSERT(actor.id != INVALID_ID, "Unit does not have actor with name '%d'", i);

	return m_world.lookup_actor(actor);
}

} // namespace crown
<|endoftext|>


<commit_before>#pragma once

#include <optional>

#include "Runtime/Collision/CCollidableAABox.hpp"
#include "Runtime/World/CActor.hpp"

#include <zeus/CAxisAngle.hpp>
#include <zeus/CQuaternion.hpp>
#include <zeus/CTransform.hpp>
#include <zeus/CVector3f.hpp>

namespace urde {
class CCollisionInfoList;
struct SMoverData;

struct SMoverData {
  zeus::CVector3f x0_velocity;
  zeus::CAxisAngle xc_angularVelocity;
  zeus::CVector3f x18_momentum;
  zeus::CAxisAngle x24_;
  float x30_mass;

  SMoverData(float mass) : x30_mass(mass) {}
};

struct CMotionState {
  zeus::CVector3f x0_translation;
  zeus::CNUQuaternion xc_orientation;
  zeus::CVector3f x1c_velocity;
  zeus::CAxisAngle x28_angularMomentum;
  CMotionState(const zeus::CVector3f& origin, const zeus::CNUQuaternion& orientation, const zeus::CVector3f& velocity,
               const zeus::CAxisAngle& angle)
  : x0_translation(origin), xc_orientation(orientation), x1c_velocity(velocity), x28_angularMomentum(angle) {}
  CMotionState(const zeus::CVector3f& origin, const zeus::CNUQuaternion& orientation)
  : x0_translation(origin), xc_orientation(orientation) {}
};

class CPhysicsState {
  zeus::CVector3f x0_translation;
  zeus::CQuaternion xc_orientation;
  zeus::CVector3f x1c_constantForce;
  zeus::CAxisAngle x28_angularMomentum;
  zeus::CVector3f x34_momentum;
  zeus::CVector3f x40_force;
  zeus::CVector3f x4c_impulse;
  zeus::CAxisAngle x58_torque;
  zeus::CAxisAngle x64_angularImpulse;

public:
  CPhysicsState(const zeus::CVector3f& translation, const zeus::CQuaternion& orient, const zeus::CVector3f& v2,
                const zeus::CAxisAngle& a1, const zeus::CVector3f& v3, const zeus::CVector3f& v4,
                const zeus::CVector3f& v5, const zeus::CAxisAngle& a2, const zeus::CAxisAngle& a3)
  : x0_translation(translation)
  , xc_orientation(orient)
  , x1c_constantForce(v2)
  , x28_angularMomentum(a1)
  , x34_momentum(v3)
  , x40_force(v4)
  , x4c_impulse(v5)
  , x58_torque(a2)
  , x64_angularImpulse(a3) {}

  void SetTranslation(const zeus::CVector3f& tr) { x0_translation = tr; }
  void SetOrientation(const zeus::CQuaternion& orient) { xc_orientation = orient; }
  const zeus::CQuaternion& GetOrientation() const { return xc_orientation; }
  const zeus::CVector3f& GetTranslation() const { return x0_translation; }
  const zeus::CVector3f& GetConstantForceWR() const { return x1c_constantForce; }
  const zeus::CAxisAngle& GetAngularMomentumWR() const { return x28_angularMomentum; }
  const zeus::CVector3f& GetMomentumWR() const { return x34_momentum; }
  const zeus::CVector3f& GetForceWR() const { return x40_force; }
  const zeus::CVector3f& GetImpulseWR() const { return x4c_impulse; }
  const zeus::CAxisAngle& GetTorque() const { return x58_torque; }
  const zeus::CAxisAngle& GetAngularImpulseWR() const { return x64_angularImpulse; }
};

class CPhysicsActor : public CActor {
  friend class CGroundMovement;

protected:
  float xe8_mass;
  float xec_massRecip;
  float xf0_inertiaTensor;
  float xf4_inertiaTensorRecip;
  union {
    struct {
      bool xf8_24_movable : 1;
      bool xf8_25_angularEnabled : 1;
    };
    u8 _dummy = 0;
  };
  bool xf9_standardCollider = false;
  zeus::CVector3f xfc_constantForce;
  zeus::CAxisAngle x108_angularMomentum;
  zeus::CMatrix3f x114_;
  zeus::CVector3f x138_velocity;
  zeus::CAxisAngle x144_angularVelocity;
  zeus::CVector3f x150_momentum;
  zeus::CVector3f x15c_force;
  zeus::CVector3f x168_impulse;
  zeus::CAxisAngle x174_torque;
  zeus::CAxisAngle x180_angularImpulse;
  zeus::CVector3f x18c_moveImpulse;
  zeus::CAxisAngle x198_moveAngularImpulse;
  zeus::CAABox x1a4_baseBoundingBox;
  CCollidableAABox x1c0_collisionPrimitive;
  zeus::CVector3f x1e8_primitiveOffset;
  CMotionState x1f4_lastNonCollidingState;
  std::optional<zeus::CVector3f> x228_lastFloorPlaneNormal;
  float x238_maximumCollisionVelocity = 1000000.0f;
  float x23c_stepUpHeight;
  float x240_stepDownHeight;
  float x244_restitutionCoefModifier = 0.f;
  float x248_collisionAccuracyModifier = 1.f;
  u32 x24c_numTicksStuck = 0;
  u32 x250_numTicksPartialUpdate = 0;

public:
  CPhysicsActor(TUniqueId, bool, std::string_view, const CEntityInfo&, const zeus::CTransform&, CModelData&&,
                const CMaterialList&, const zeus::CAABox&, const SMoverData&, const CActorParameters&, float, float);

  void Render(const CStateManager& mgr) const override;
  zeus::CVector3f GetOrbitPosition(const CStateManager&) const override;
  zeus::CVector3f GetAimPosition(const CStateManager&, float val) const override;
  virtual const CCollisionPrimitive* GetCollisionPrimitive() const;
  virtual zeus::CTransform GetPrimitiveTransform() const;
  virtual void CollidedWith(TUniqueId, const CCollisionInfoList&, CStateManager&);
  virtual float GetStepUpHeight() const;
  virtual float GetStepDownHeight() const;
  virtual float GetWeight() const;

  float GetMass() const { return xe8_mass; }
  void SetPrimitiveOffset(const zeus::CVector2f& offset);
  const zeus::CVector3f& GetPrimitiveOffset() const { return x1e8_primitiveOffset; }
  void MoveCollisionPrimitive(const zeus::CVector3f& offset);
  void SetBoundingBox(const zeus::CAABox& box);
  zeus::CAABox GetMotionVolume(float dt) const;
  zeus::CVector3f CalculateNewVelocityWR_UsingImpulses() const;
  zeus::CAABox GetBoundingBox() const;
  const zeus::CAABox& GetBaseBoundingBox() const;
  void AddMotionState(const CMotionState& mst);
  CMotionState GetMotionState() const;
  const CMotionState& GetLastNonCollidingState() const { return x1f4_lastNonCollidingState; }
  void SetLastNonCollidingState(const CMotionState& mst) { x1f4_lastNonCollidingState = mst; }
  void SetMotionState(const CMotionState& mst);
  float GetMaximumCollisionVelocity() const { return x238_maximumCollisionVelocity; }
  void SetMaximumCollisionVelocity(float v) { x238_maximumCollisionVelocity = v; }
  void SetInertiaTensorScalar(float tensor);
  void SetMass(float mass);
  void SetAngularVelocityOR(const zeus::CAxisAngle& angVel);
  zeus::CAxisAngle GetAngularVelocityOR() const;
  const zeus::CAxisAngle& GetAngularVelocityWR() const { return x144_angularVelocity; }
  void SetAngularVelocityWR(const zeus::CAxisAngle& angVel);
  const zeus::CVector3f& GetForceOR() const { return x15c_force; }
  const zeus::CVector3f& GetImpulseOR() const { return x168_impulse; }
  const zeus::CVector3f& GetMoveImpulseOR() const { return x18c_moveImpulse; }
  void SetVelocityWR(const zeus::CVector3f& vel);
  void SetVelocityOR(const zeus::CVector3f& vel);
  void SetMomentumWR(const zeus::CVector3f& m) { x150_momentum = m; }
  const zeus::CVector3f& GetConstantForce() const { return xfc_constantForce; }
  void SetConstantForce(const zeus::CVector3f& f) { xfc_constantForce = f; }
  void SetAngularMomentum(const zeus::CAxisAngle& m) { x108_angularMomentum = m; }
  const zeus::CVector3f& GetMomentum() const { return x150_momentum; }
  const zeus::CVector3f& GetVelocity() const { return x138_velocity; }
  const zeus::CAxisAngle& GetAngularImpulse() const { return x180_angularImpulse; }
  void SetAngularImpulse(const zeus::CAxisAngle& i) { x180_angularImpulse = i; }
  zeus::CVector3f GetTotalForcesWR() const;
  void RotateInOneFrameOR(const zeus::CQuaternion& q, float d);
  void MoveInOneFrameOR(const zeus::CVector3f& trans, float d);
  void RotateToOR(const zeus::CQuaternion& q, float d);
  void MoveToOR(const zeus::CVector3f& trans, float d);
  void MoveToInOneFrameWR(const zeus::CVector3f& v1, float d);
  void MoveToWR(const zeus::CVector3f& trans, float d);
  zeus::CAxisAngle GetRotateToORAngularMomentumWR(const zeus::CQuaternion& q, float d) const;
  zeus::CVector3f GetMoveToORImpulseWR(const zeus::CVector3f& trans, float d) const;
  void ClearImpulses();
  void ClearForcesAndTorques();
  void Stop();
  void ComputeDerivedQuantities();
  bool WillMove(const CStateManager&) const;
  void SetPhysicsState(const CPhysicsState& state);
  CPhysicsState GetPhysicsState() const;
  bool IsMovable() const { return xf8_24_movable; }
  void SetMovable(bool m) { xf8_24_movable = m; }
  bool IsAngularEnabled() const { return xf8_25_angularEnabled; }
  void SetAngularEnabled(bool e) { xf8_25_angularEnabled = e; }
  float GetCollisionAccuracyModifier() const { return x248_collisionAccuracyModifier; }
  void SetCollisionAccuracyModifier(float m) { x248_collisionAccuracyModifier = m; }
  float GetCoefficientOfRestitutionModifier() const { return x244_restitutionCoefModifier; }
  void SetCoefficientOfRestitutionModifier(float m) { x244_restitutionCoefModifier = m; }
  bool IsUseStandardCollider() const { return xf9_standardCollider; }
  u32 GetNumTicksPartialUpdate() const { return x250_numTicksPartialUpdate; }
  void SetNumTicksPartialUpdate(u32 t) { x250_numTicksPartialUpdate = t; }
  u32 GetNumTicksStuck() const { return x24c_numTicksStuck; }
  void SetNumTicksStuck(u32 t) { x24c_numTicksStuck = t; }
  const std::optional<zeus::CVector3f>& GetLastFloorPlaneNormal() const {
    return x228_lastFloorPlaneNormal;
  }
  void SetLastFloorPlaneNormal(const std::optional<zeus::CVector3f>& n) { x228_lastFloorPlaneNormal = n; }

  CMotionState PredictMotion_Internal(float) const;
  CMotionState PredictMotion(float dt) const;
  CMotionState PredictLinearMotion(float dt) const;
  CMotionState PredictAngularMotion(float dt) const;
  void ApplyForceOR(const zeus::CVector3f& force, const zeus::CAxisAngle& angle);
  void ApplyForceWR(const zeus::CVector3f& force, const zeus::CAxisAngle& angle);
  void ApplyImpulseOR(const zeus::CVector3f& impulse, const zeus::CAxisAngle& angle);
  void ApplyImpulseWR(const zeus::CVector3f& impulse, const zeus::CAxisAngle& angle);
  void ApplyTorqueWR(const zeus::CVector3f& torque);

  void UseCollisionImpulses();
  static constexpr float GravityConstant() { return 9.81f * 2.5f; } /* 9.81 m/s ^ 2 is normal acceleration under earth gravity, Tallon 4 is 2.5 times that */
};
} // namespace urde
<commit_msg>CPhysicsActor: Add names to parameters in prototypes<commit_after>#pragma once

#include <optional>

#include "Runtime/Collision/CCollidableAABox.hpp"
#include "Runtime/World/CActor.hpp"

#include <zeus/CAxisAngle.hpp>
#include <zeus/CQuaternion.hpp>
#include <zeus/CTransform.hpp>
#include <zeus/CVector3f.hpp>

namespace urde {
class CCollisionInfoList;
struct SMoverData;

struct SMoverData {
  zeus::CVector3f x0_velocity;
  zeus::CAxisAngle xc_angularVelocity;
  zeus::CVector3f x18_momentum;
  zeus::CAxisAngle x24_;
  float x30_mass;

  SMoverData(float mass) : x30_mass(mass) {}
};

struct CMotionState {
  zeus::CVector3f x0_translation;
  zeus::CNUQuaternion xc_orientation;
  zeus::CVector3f x1c_velocity;
  zeus::CAxisAngle x28_angularMomentum;
  CMotionState(const zeus::CVector3f& origin, const zeus::CNUQuaternion& orientation, const zeus::CVector3f& velocity,
               const zeus::CAxisAngle& angle)
  : x0_translation(origin), xc_orientation(orientation), x1c_velocity(velocity), x28_angularMomentum(angle) {}
  CMotionState(const zeus::CVector3f& origin, const zeus::CNUQuaternion& orientation)
  : x0_translation(origin), xc_orientation(orientation) {}
};

class CPhysicsState {
  zeus::CVector3f x0_translation;
  zeus::CQuaternion xc_orientation;
  zeus::CVector3f x1c_constantForce;
  zeus::CAxisAngle x28_angularMomentum;
  zeus::CVector3f x34_momentum;
  zeus::CVector3f x40_force;
  zeus::CVector3f x4c_impulse;
  zeus::CAxisAngle x58_torque;
  zeus::CAxisAngle x64_angularImpulse;

public:
  CPhysicsState(const zeus::CVector3f& translation, const zeus::CQuaternion& orient, const zeus::CVector3f& v2,
                const zeus::CAxisAngle& a1, const zeus::CVector3f& v3, const zeus::CVector3f& v4,
                const zeus::CVector3f& v5, const zeus::CAxisAngle& a2, const zeus::CAxisAngle& a3)
  : x0_translation(translation)
  , xc_orientation(orient)
  , x1c_constantForce(v2)
  , x28_angularMomentum(a1)
  , x34_momentum(v3)
  , x40_force(v4)
  , x4c_impulse(v5)
  , x58_torque(a2)
  , x64_angularImpulse(a3) {}

  void SetTranslation(const zeus::CVector3f& tr) { x0_translation = tr; }
  void SetOrientation(const zeus::CQuaternion& orient) { xc_orientation = orient; }
  const zeus::CQuaternion& GetOrientation() const { return xc_orientation; }
  const zeus::CVector3f& GetTranslation() const { return x0_translation; }
  const zeus::CVector3f& GetConstantForceWR() const { return x1c_constantForce; }
  const zeus::CAxisAngle& GetAngularMomentumWR() const { return x28_angularMomentum; }
  const zeus::CVector3f& GetMomentumWR() const { return x34_momentum; }
  const zeus::CVector3f& GetForceWR() const { return x40_force; }
  const zeus::CVector3f& GetImpulseWR() const { return x4c_impulse; }
  const zeus::CAxisAngle& GetTorque() const { return x58_torque; }
  const zeus::CAxisAngle& GetAngularImpulseWR() const { return x64_angularImpulse; }
};

class CPhysicsActor : public CActor {
  friend class CGroundMovement;

protected:
  float xe8_mass;
  float xec_massRecip;
  float xf0_inertiaTensor;
  float xf4_inertiaTensorRecip;
  union {
    struct {
      bool xf8_24_movable : 1;
      bool xf8_25_angularEnabled : 1;
    };
    u8 _dummy = 0;
  };
  bool xf9_standardCollider = false;
  zeus::CVector3f xfc_constantForce;
  zeus::CAxisAngle x108_angularMomentum;
  zeus::CMatrix3f x114_;
  zeus::CVector3f x138_velocity;
  zeus::CAxisAngle x144_angularVelocity;
  zeus::CVector3f x150_momentum;
  zeus::CVector3f x15c_force;
  zeus::CVector3f x168_impulse;
  zeus::CAxisAngle x174_torque;
  zeus::CAxisAngle x180_angularImpulse;
  zeus::CVector3f x18c_moveImpulse;
  zeus::CAxisAngle x198_moveAngularImpulse;
  zeus::CAABox x1a4_baseBoundingBox;
  CCollidableAABox x1c0_collisionPrimitive;
  zeus::CVector3f x1e8_primitiveOffset;
  CMotionState x1f4_lastNonCollidingState;
  std::optional<zeus::CVector3f> x228_lastFloorPlaneNormal;
  float x238_maximumCollisionVelocity = 1000000.0f;
  float x23c_stepUpHeight;
  float x240_stepDownHeight;
  float x244_restitutionCoefModifier = 0.f;
  float x248_collisionAccuracyModifier = 1.f;
  u32 x24c_numTicksStuck = 0;
  u32 x250_numTicksPartialUpdate = 0;

public:
  CPhysicsActor(TUniqueId uid, bool active, std::string_view name, const CEntityInfo& info, const zeus::CTransform& xf,
                CModelData&& mData, const CMaterialList& matList, const zeus::CAABox& box, const SMoverData& moverData,
                const CActorParameters& actorParms, float stepUp, float stepDown);

  void Render(const CStateManager& mgr) const override;
  zeus::CVector3f GetOrbitPosition(const CStateManager& mgr) const override;
  zeus::CVector3f GetAimPosition(const CStateManager& mgr, float val) const override;
  virtual const CCollisionPrimitive* GetCollisionPrimitive() const;
  virtual zeus::CTransform GetPrimitiveTransform() const;
  virtual void CollidedWith(TUniqueId id, const CCollisionInfoList& list, CStateManager& mgr);
  virtual float GetStepUpHeight() const;
  virtual float GetStepDownHeight() const;
  virtual float GetWeight() const;

  float GetMass() const { return xe8_mass; }
  void SetPrimitiveOffset(const zeus::CVector2f& offset);
  const zeus::CVector3f& GetPrimitiveOffset() const { return x1e8_primitiveOffset; }
  void MoveCollisionPrimitive(const zeus::CVector3f& offset);
  void SetBoundingBox(const zeus::CAABox& box);
  zeus::CAABox GetMotionVolume(float dt) const;
  zeus::CVector3f CalculateNewVelocityWR_UsingImpulses() const;
  zeus::CAABox GetBoundingBox() const;
  const zeus::CAABox& GetBaseBoundingBox() const;
  void AddMotionState(const CMotionState& mst);
  CMotionState GetMotionState() const;
  const CMotionState& GetLastNonCollidingState() const { return x1f4_lastNonCollidingState; }
  void SetLastNonCollidingState(const CMotionState& mst) { x1f4_lastNonCollidingState = mst; }
  void SetMotionState(const CMotionState& mst);
  float GetMaximumCollisionVelocity() const { return x238_maximumCollisionVelocity; }
  void SetMaximumCollisionVelocity(float velocity) { x238_maximumCollisionVelocity = velocity; }
  void SetInertiaTensorScalar(float tensor);
  void SetMass(float mass);
  void SetAngularVelocityOR(const zeus::CAxisAngle& angVel);
  zeus::CAxisAngle GetAngularVelocityOR() const;
  const zeus::CAxisAngle& GetAngularVelocityWR() const { return x144_angularVelocity; }
  void SetAngularVelocityWR(const zeus::CAxisAngle& angVel);
  const zeus::CVector3f& GetForceOR() const { return x15c_force; }
  const zeus::CVector3f& GetImpulseOR() const { return x168_impulse; }
  const zeus::CVector3f& GetMoveImpulseOR() const { return x18c_moveImpulse; }
  void SetVelocityWR(const zeus::CVector3f& vel);
  void SetVelocityOR(const zeus::CVector3f& vel);
  void SetMomentumWR(const zeus::CVector3f& momentum) { x150_momentum = momentum; }
  const zeus::CVector3f& GetConstantForce() const { return xfc_constantForce; }
  void SetConstantForce(const zeus::CVector3f& force) { xfc_constantForce = force; }
  void SetAngularMomentum(const zeus::CAxisAngle& momentum) { x108_angularMomentum = momentum; }
  const zeus::CVector3f& GetMomentum() const { return x150_momentum; }
  const zeus::CVector3f& GetVelocity() const { return x138_velocity; }
  const zeus::CAxisAngle& GetAngularImpulse() const { return x180_angularImpulse; }
  void SetAngularImpulse(const zeus::CAxisAngle& impulse) { x180_angularImpulse = impulse; }
  zeus::CVector3f GetTotalForcesWR() const;
  void RotateInOneFrameOR(const zeus::CQuaternion& q, float d);
  void MoveInOneFrameOR(const zeus::CVector3f& trans, float d);
  void RotateToOR(const zeus::CQuaternion& q, float d);
  void MoveToOR(const zeus::CVector3f& trans, float d);
  void MoveToInOneFrameWR(const zeus::CVector3f& v1, float d);
  void MoveToWR(const zeus::CVector3f& trans, float d);
  zeus::CAxisAngle GetRotateToORAngularMomentumWR(const zeus::CQuaternion& q, float d) const;
  zeus::CVector3f GetMoveToORImpulseWR(const zeus::CVector3f& trans, float d) const;
  void ClearImpulses();
  void ClearForcesAndTorques();
  void Stop();
  void ComputeDerivedQuantities();
  bool WillMove(const CStateManager& mgr) const;
  void SetPhysicsState(const CPhysicsState& state);
  CPhysicsState GetPhysicsState() const;
  bool IsMovable() const { return xf8_24_movable; }
  void SetMovable(bool movable) { xf8_24_movable = movable; }
  bool IsAngularEnabled() const { return xf8_25_angularEnabled; }
  void SetAngularEnabled(bool enabled) { xf8_25_angularEnabled = enabled; }
  float GetCollisionAccuracyModifier() const { return x248_collisionAccuracyModifier; }
  void SetCollisionAccuracyModifier(float modifier) { x248_collisionAccuracyModifier = modifier; }
  float GetCoefficientOfRestitutionModifier() const { return x244_restitutionCoefModifier; }
  void SetCoefficientOfRestitutionModifier(float modifier) { x244_restitutionCoefModifier = modifier; }
  bool IsUseStandardCollider() const { return xf9_standardCollider; }
  u32 GetNumTicksPartialUpdate() const { return x250_numTicksPartialUpdate; }
  void SetNumTicksPartialUpdate(u32 ticks) { x250_numTicksPartialUpdate = ticks; }
  u32 GetNumTicksStuck() const { return x24c_numTicksStuck; }
  void SetNumTicksStuck(u32 ticks) { x24c_numTicksStuck = ticks; }
  const std::optional<zeus::CVector3f>& GetLastFloorPlaneNormal() const {
    return x228_lastFloorPlaneNormal;
  }
  void SetLastFloorPlaneNormal(const std::optional<zeus::CVector3f>& normal) { x228_lastFloorPlaneNormal = normal; }

  CMotionState PredictMotion_Internal(float) const;
  CMotionState PredictMotion(float dt) const;
  CMotionState PredictLinearMotion(float dt) const;
  CMotionState PredictAngularMotion(float dt) const;
  void ApplyForceOR(const zeus::CVector3f& force, const zeus::CAxisAngle& angle);
  void ApplyForceWR(const zeus::CVector3f& force, const zeus::CAxisAngle& angle);
  void ApplyImpulseOR(const zeus::CVector3f& impulse, const zeus::CAxisAngle& angle);
  void ApplyImpulseWR(const zeus::CVector3f& impulse, const zeus::CAxisAngle& angle);
  void ApplyTorqueWR(const zeus::CVector3f& torque);

  void UseCollisionImpulses();
  static constexpr float GravityConstant() { return 9.81f * 2.5f; } /* 9.81 m/s ^ 2 is normal acceleration under earth gravity, Tallon 4 is 2.5 times that */
};
} // namespace urde
<|endoftext|>


<commit_before>#include <HgModel.h>
#include <stdio.h>
#include <stdlib.h>
#include <HgVbo.h>

#include "oglDisplay.h"

#include <ini.h>

typedef struct header {
	uint32_t vertex_count, index_count;
} header;

static model_data LoadModel(const char* filename) {
	header head;
	model_data r;
	r.vertices = NULL;
	r.indices = NULL;

	vbo_layout_vnu* buffer1 = NULL;
	uint16_t* buffer2 = NULL;

	FILE* f = NULL;
	errno_t err = fopen_s(&f, filename, "rb");
	if (err != 0) {
		fprintf(stderr, "Unable to open file \"%s\"\n", filename);
		return r;
	}

	int32_t read = fread(&head, sizeof(head), 1, f);
	if (read != 1) {
		fprintf(stderr, "Unable to read file header for \"%s\"\n", filename);
		fclose(f);
		return r;
	}

	if (head.vertex_count > 65535) {
		fprintf(stderr, "Too many vertices for \"%s\"\n", filename);
		fclose(f);
		return r;
	}

	if (head.index_count > 1000000) {
		fprintf(stderr, "Too many indices for \"%s\"\n", filename);
		fclose(f);
		return r;
	}

	buffer1 = (vbo_layout_vnu*)malloc(sizeof(*buffer1)*head.vertex_count);
	buffer2 = (uint16_t*)malloc(sizeof(*buffer2)*head.index_count);

	read = fread(buffer1, sizeof(*buffer1), head.vertex_count, f);
	if (read != head.vertex_count) {
		fprintf(stderr, "Error, %d vertices expected, read %d", head.vertex_count, read);
		free(buffer1);
		free(buffer2);
		fclose(f);
		return r;
	}

	read = fread(buffer2, sizeof(*buffer2), head.index_count, f);
	if (read != head.index_count) {
		fprintf(stderr, "Error, %d indices expected, read %d", head.index_count, read);
		free(buffer1);
		free(buffer2);
		fclose(f);
		return r;
	}

	fclose(f);

	r.vertices = buffer1;
	r.indices = buffer2;
	r.vertex_count = head.vertex_count;
	r.index_count = head.index_count;

	return r;
}

static RenderData* init_render_data() {
	OGLRenderData* rd = OGLRenderData::Create();
//	rd->baseRender.renderFunc = model_render;
	return rd;
}

static void updateClbk(HgElement* e, uint32_t tdelta) {
	//	printf("cube\n");
}

model_data::model_data()
	:vertices(nullptr), indices(nullptr)
{

}
model_data::~model_data() {
}

static void destroy(HgElement* e) {
//	SCALL(e->m_renderData, destroy);
	free(e->m_renderData);
	e->m_renderData = NULL;
}

static void change_to_model(HgElement* element) {
//	element->vptr_idx = VTABLE_INDEX;

	//create an instance of the render data for all triangles to share
	element->m_renderData = init_render_data(); //this needs to be per model instance if the model is animated
}

int8_t model_load(HgElement* element, const char* filename) {
	change_to_model(element);

	OGLRenderData* rd = (OGLRenderData*)element->m_renderData;

	SET_FLAG(element, HGE_DESTROY); //clear when we make it to the end

	model_data mdl = LoadModel(filename);
	if (mdl.vertices == NULL || mdl.indices == NULL) return -1;

	rd->hgVbo = &staticVboVNU;
	rd->vertex_count = mdl.vertex_count;
	rd->index_count = mdl.index_count;
	rd->vbo_offset = staticVboVNU.add_data(mdl.vertices, rd->vertex_count);
	free(mdl.vertices);

//	mrd->index_count = mdl.index_count;
	rd->indices.data = mdl.indices;
	rd->indices.owns_ptr = 1;
	rd->renderFunction = Indice16Render;

	CLEAR_FLAG(element, HGE_DESTROY);

	return 0;
}

typedef struct iniData{
	std::string modeFilename;
	float scale;
	vector3 origin;
	quaternion orientation;
	std::string vertexShader;
	std::string fragmentShader;
} iniData;

static int iniHandler(void* user, const char* section, const char* name, const char* value) {
	iniData* data = (iniData*)user;

	#define MATCH(s, n) strcmp(section, s) == 0 && strcmp(name, n) == 0

	if (MATCH("Model", "file")) {
		data->modeFilename = value;
	}
	else if (MATCH("Model", "scale")) {
		data->scale = ::atof(value);
	}
	else if (MATCH("Model", "origin")) {
		float x, y, z;
		int r = sscanf(value, "%f,%f,%f", &x, &y, &z);
		if (r == 3) {
			data->origin = { x,y,z };
		}
		else {
			//warn
		}
	}
	else if (MATCH("Model", "orientation")) {
		float x, y, z;
		int r = sscanf(value, "%f,%f,%f", &x, &y, &z);
		if (r == 3) {
			data->orientation = toQuaternion2(y, x, z); //y,x,z
		}
		else {
			//warn
		}
	}
	else if (MATCH("Model", "vertexShader")) {
		data->vertexShader = value;
	}
	else if (MATCH("Model", "fragmentShader")) {
		data->fragmentShader = value;
	}
	return 0;
}

bool model_load_ini(HgElement* element, std::string filename) {
	change_to_model(element);
	iniData data;

	ini_parse(filename.c_str(), iniHandler, &data);

	if (model_load(element, data.modeFilename.c_str()) < 0) return false;

	element->scale = data.scale;
	element->origin.components.z = data.origin.components.z / element->scale;
	element->origin.components.y = data.origin.components.y / element->scale;
	element->origin.components.x = data.origin.components.x / element->scale;
	element->rotation = data.orientation;

	if (!data.vertexShader.empty() && !data.fragmentShader.empty())
		element->m_renderData->shader = HgShader::acquire(data.vertexShader.c_str(), data.fragmentShader.c_str());

	return true;
}

/*
void model_create(HgElement* element) {
	element->position.components.x = 0.0f;
	element->position.components.y = 0.0f;
	element->position.components.z = 0.0f;

	element->rotation.w = 1.0f;
	//	element->rotation.z = 0.707f;

	element->scale = 1;

	change_to_model(element);
}
*/

REGISTER_LINKTIME(hgmodel,change_to_model);<commit_msg>add position to ini fix initial values<commit_after>#include <HgModel.h>
#include <stdio.h>
#include <stdlib.h>
#include <HgVbo.h>

#include "oglDisplay.h"

#include <ini.h>

typedef struct header {
	uint32_t vertex_count, index_count;
} header;

static model_data LoadModel(const char* filename) {
	header head;
	model_data r;
	r.vertices = NULL;
	r.indices = NULL;

	vbo_layout_vnu* buffer1 = NULL;
	uint16_t* buffer2 = NULL;

	FILE* f = NULL;
	errno_t err = fopen_s(&f, filename, "rb");
	if (err != 0) {
		fprintf(stderr, "Unable to open file \"%s\"\n", filename);
		return r;
	}

	int32_t read = fread(&head, sizeof(head), 1, f);
	if (read != 1) {
		fprintf(stderr, "Unable to read file header for \"%s\"\n", filename);
		fclose(f);
		return r;
	}

	if (head.vertex_count > 65535) {
		fprintf(stderr, "Too many vertices for \"%s\"\n", filename);
		fclose(f);
		return r;
	}

	if (head.index_count > 1000000) {
		fprintf(stderr, "Too many indices for \"%s\"\n", filename);
		fclose(f);
		return r;
	}

	buffer1 = (vbo_layout_vnu*)malloc(sizeof(*buffer1)*head.vertex_count);
	buffer2 = (uint16_t*)malloc(sizeof(*buffer2)*head.index_count);

	read = fread(buffer1, sizeof(*buffer1), head.vertex_count, f);
	if (read != head.vertex_count) {
		fprintf(stderr, "Error, %d vertices expected, read %d", head.vertex_count, read);
		free(buffer1);
		free(buffer2);
		fclose(f);
		return r;
	}

	read = fread(buffer2, sizeof(*buffer2), head.index_count, f);
	if (read != head.index_count) {
		fprintf(stderr, "Error, %d indices expected, read %d", head.index_count, read);
		free(buffer1);
		free(buffer2);
		fclose(f);
		return r;
	}

	fclose(f);

	r.vertices = buffer1;
	r.indices = buffer2;
	r.vertex_count = head.vertex_count;
	r.index_count = head.index_count;

	return r;
}

static RenderData* init_render_data() {
	OGLRenderData* rd = OGLRenderData::Create();
//	rd->baseRender.renderFunc = model_render;
	return rd;
}

static void updateClbk(HgElement* e, uint32_t tdelta) {
	//	printf("cube\n");
}

model_data::model_data()
	:vertices(nullptr), indices(nullptr)
{

}
model_data::~model_data() {
}

static void destroy(HgElement* e) {
//	SCALL(e->m_renderData, destroy);
	free(e->m_renderData);
	e->m_renderData = NULL;
}

static void change_to_model(HgElement* element) {
//	element->vptr_idx = VTABLE_INDEX;

	//create an instance of the render data for all triangles to share
	element->m_renderData = init_render_data(); //this needs to be per model instance if the model is animated
}

int8_t model_load(HgElement* element, const char* filename) {
	change_to_model(element);

	OGLRenderData* rd = (OGLRenderData*)element->m_renderData;

	SET_FLAG(element, HGE_DESTROY); //clear when we make it to the end

	model_data mdl = LoadModel(filename);
	if (mdl.vertices == NULL || mdl.indices == NULL) return -1;

	rd->hgVbo = &staticVboVNU;
	rd->vertex_count = mdl.vertex_count;
	rd->index_count = mdl.index_count;
	rd->vbo_offset = staticVboVNU.add_data(mdl.vertices, rd->vertex_count);
	free(mdl.vertices);

//	mrd->index_count = mdl.index_count;
	rd->indices.data = mdl.indices;
	rd->indices.owns_ptr = 1;
	rd->renderFunction = Indice16Render;

	CLEAR_FLAG(element, HGE_DESTROY);

	return 0;
}

typedef struct iniData{
	iniData() {
		scale = 1.0f;
		origin = position = { 0,0,0 };
	}
	std::string modeFilename;
	float scale;
	vector3 origin;
	vector3 position;
	quaternion orientation;
	std::string vertexShader;
	std::string fragmentShader;
} iniData;

static int iniHandler(void* user, const char* section, const char* name, const char* value) {
	iniData* data = (iniData*)user;

	#define MATCH(s, n) strcmp(section, s) == 0 && strcmp(name, n) == 0

	if (MATCH("Model", "file")) {
		data->modeFilename = value;
	}
	else if (MATCH("Model", "scale")) {
		data->scale = ::atof(value);
	}
	else if (MATCH("Model", "origin")) {
		float x, y, z;
		int r = sscanf(value, "%f,%f,%f", &x, &y, &z);
		if (r == 3) {
			data->origin = { x,y,z };
		}
		else {
			//warn
		}
	}
	else if (MATCH("Model", "position")) {
		float x, y, z;
		int r = sscanf(value, "%f,%f,%f", &x, &y, &z);
		if (r == 3) {
			data->position = { x,y,z };
		}
		else {
			//warn
		}
	}
	else if (MATCH("Model", "orientation")) {
		float x, y, z;
		int r = sscanf(value, "%f,%f,%f", &x, &y, &z);
		if (r == 3) {
			data->orientation = toQuaternion2(y, x, z); //y,x,z
		}
		else {
			//warn
		}
	}
	else if (MATCH("Model", "vertexShader")) {
		data->vertexShader = value;
	}
	else if (MATCH("Model", "fragmentShader")) {
		data->fragmentShader = value;
	}
	return 0;
}

bool model_load_ini(HgElement* element, std::string filename) {
	change_to_model(element);
	iniData data;

	ini_parse(filename.c_str(), iniHandler, &data);

	if (model_load(element, data.modeFilename.c_str()) < 0) return false;

	element->scale = data.scale;
	element->origin.components.z = data.origin.components.z / element->scale;
	element->origin.components.y = data.origin.components.y / element->scale;
	element->origin.components.x = data.origin.components.x / element->scale;
	element->position = data.position;
	element->rotation = data.orientation;

	if (!data.vertexShader.empty() && !data.fragmentShader.empty())
		element->m_renderData->shader = HgShader::acquire(data.vertexShader.c_str(), data.fragmentShader.c_str());

	return true;
}

/*
void model_create(HgElement* element) {
	element->position.components.x = 0.0f;
	element->position.components.y = 0.0f;
	element->position.components.z = 0.0f;

	element->rotation.w = 1.0f;
	//	element->rotation.z = 0.707f;

	element->scale = 1;

	change_to_model(element);
}
*/

REGISTER_LINKTIME(hgmodel,change_to_model);<|endoftext|>


<commit_before>//
//
// MIT License
//
// Copyright (c) 2020 Stellacore Corporation.
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal in the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject
// to the following conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY
// KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN
// AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR
// IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//
//

/*! \file
\brief  This file contains unit test for cam::fit
*/

#include "libcam/fit.h"

#include "libga/spin.h"
#include "libgeo/VertGangle.h"

#include "libdat/info.h"
#include "libdat/ops.h"
#include "libdat/validity.h"
#include "libio/stream.h"

#include <iostream>
#include <memory>
#include <sstream>
#include <string>

namespace
{

//! Check for common functions
std::string
cam_fit_test0
	()
{
	std::ostringstream oss;
	/*
	cam::fit const aNull(dat::nullValue<cam::fit>());
	if (dat::isValid(aNull))
	{
		oss << "Failure of null value test" << std::endl;
		oss << "infoString: " << dat::infoString(aNull) << std::endl;
	}
	*/
	return oss.str();
}

namespace
{
	//! Interior geometry for particular test case
	struct Interior
	{
		dat::Extents const theDetSize;
		dat::Spot const theCenterRC;
		ga::Vector const theCenterVec;

		Interior
			( dat::Extents const & detSize
			)
			: theDetSize{ detSize }
			, theCenterRC{ dat::centerOf(theDetSize) }
			, theCenterVec{ theCenterRC[0], theCenterRC[1], 0. }
		{ }

		geo::VertGangle
		gangleFor
			( double const & pd
			, std::pair<dat::Spot, dat::Spot> const & meaPair
			) const
		{
			dat::Spot const & meaRowCol1 = meaPair.first;
			dat::Spot const & meaRowCol2 = meaPair.second;
			ga::Vector const vecMea1{ meaRowCol1[0], meaRowCol1[1], 0. };
			ga::Vector const vecMea2{ meaRowCol2[0], meaRowCol2[1], 0. };
			ga::Vector const vecPD{ theCenterVec + pd * ga::e3 };
			return geo::VertGangle{ vecPD, std::make_pair(vecMea1, vecMea2) };
		}

		void
		showData
			( std::pair<dat::Spot, dat::Spot> const & meaPair
			) const
		{
			for (double pd{0.} ; pd < 250. ; pd += 10.)
			{
				geo::VertGangle const gangle{ gangleFor(pd, meaPair) };
				io::out()
					<< " pd: " << dat::infoString(pd)
					<< " gangle: " << dat::infoString(gangle)
					<< std::endl;
			}
		}

	}; // Interior

}

//! Check principal distance calibration
std::string
cam_fit_test1
	()
{
	std::ostringstream oss;

	// simulate a detector
	dat::Extents const detSize{ 2000u, 3000u };
	dat::Spot const rcCenter{ dat::centerOf(detSize) };
	ga::Vector const center{ rcCenter[0], rcCenter[1], 0. };

	double const expPD{ 100. };
	double const delta{ expPD };
	using dat::operator+;
	using dat::operator-;
	dat::Spot const meaRowCol1{ rcCenter + dat::Spot{ delta, 500. } };
	dat::Spot const meaRowCol2{ rcCenter - dat::Spot{ delta, 500. } };

	std::pair<dat::Spot, dat::Spot> const meaPair{ meaRowCol1, meaRowCol2 };
	ga::Vector const vecMea1{ meaRowCol1[0], meaRowCol1[1], 0. };
	ga::Vector const vecMea2{ meaRowCol2[0], meaRowCol2[1], 0. };
	ga::Vector const vecPD{ center + expPD * ga::e3 };

	geo::VertGangle const imgGangle{ vecPD, std::make_pair(vecMea1, vecMea2) };
	double const beta{ imgGangle.angleMag() };
	std::vector<double> const gotPDs
		{ cam::fit::principalDistanceFor(meaPair, beta, detSize) };

	// display all solutions
	io::out() << dat::infoString(gotPDs.begin(), gotPDs.end(), "expPD") << '\n';
	io::out() << dat::infoString(beta, "beta") << '\n';
	for (double const & gotPD : gotPDs)
	{
		io::out() << io::sprintf("... %25.18f", gotPD) << std::endl;
	}

	// display expected profile
	Interior const inner(detSize);
	inner.showData(meaPair);

	bool okay{ false };
	double const tol{ dat::diagonalMag(detSize) * math::eps };
	for (double const & gotPD : gotPDs)
	{
		if (dat::nearlyEquals(gotPD, expPD, tol))
		{
			okay = true;
		}
	}

	if (! okay)
	{
		oss << "Failure to recover principal distance test" << std::endl;
		oss << dat::infoString(expPD, "expPD") << std::endl;
		for (double const & gotPD : gotPDs)
		{
			oss << io::sprintf("... %25.18f", gotPD) << std::endl;
		}
		oss << dat::infoString(meaRowCol1, "meaRowCol1") << std::endl;
		oss << dat::infoString(meaRowCol2, "meaRowCol2") << std::endl;
		oss << dat::infoString(beta, "beta") << std::endl;
	}

	return oss.str();
}

}

//! Unit test for cam::fit
int
main
	( int const /*argc*/
	, char const * const * /*argv*/
	)
{
	std::ostringstream oss;

	// run tests
	oss << cam_fit_test0();
	oss << cam_fit_test1();

	// check/report results
	std::string const errMessages(oss.str());
	if (! errMessages.empty())
	{
		io::err() << errMessages << std::endl;
		return 1;
	}
	return 0;
}
<commit_msg>testcam/ufit full test looping over various combinations of grid locations<commit_after>//
//
// MIT License
//
// Copyright (c) 2020 Stellacore Corporation.
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal in the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject
// to the following conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY
// KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN
// AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR
// IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//
//

/*! \file
\brief  This file contains unit test for cam::fit
*/

#include "libcam/fit.h"

#include "libga/spin.h"
#include "libgeo/VertGangle.h"

#include "libdat/info.h"
#include "libdat/ops.h"
#include "libdat/validity.h"
#include "libio/stream.h"

#include <iostream>
#include <memory>
#include <sstream>
#include <string>

namespace
{

//! Check for common functions
std::string
cam_fit_test0
	()
{
	std::ostringstream oss;
	/*
	cam::fit const aNull(dat::nullValue<cam::fit>());
	if (dat::isValid(aNull))
	{
		oss << "Failure of null value test" << std::endl;
		oss << "infoString: " << dat::infoString(aNull) << std::endl;
	}
	*/
	return oss.str();
}

namespace
{
	//! Interior geometry for particular test case
	struct Interior
	{
		dat::Extents const theDetSize;
		dat::Spot const theCenterRC;
		ga::Vector const theCenterVec;

		Interior
			( dat::Extents const & detSize
			)
			: theDetSize{ detSize }
			, theCenterRC{ dat::centerOf(theDetSize) }
			, theCenterVec{ theCenterRC[0], theCenterRC[1], 0. }
		{ }

		geo::VertGangle
		gangleFor
			( double const & pd
			, std::pair<dat::Spot, dat::Spot> const & meaPair
			) const
		{
			dat::Spot const & meaRowCol1 = meaPair.first;
			dat::Spot const & meaRowCol2 = meaPair.second;
			ga::Vector const vecMea1{ meaRowCol1[0], meaRowCol1[1], 0. };
			ga::Vector const vecMea2{ meaRowCol2[0], meaRowCol2[1], 0. };
			ga::Vector const vecPD{ theCenterVec + pd * ga::e3 };
			return geo::VertGangle{ vecPD, std::make_pair(vecMea1, vecMea2) };
		}

		double
		beta
			( double const & pd
			, std::pair<dat::Spot, dat::Spot> const & meaPair
			) const
		{
			return gangleFor(pd, meaPair).angleMag();
		}

		std::string
		infoProfile
			( std::pair<dat::Spot, dat::Spot> const & meaPair
			, double const & minPD = 0.
			, double const & maxPD = 250.
			, double const & delPD = 10.
			) const
		{
			std::ostringstream oss;
			for (double pd{minPD} ; (! (maxPD < pd)) ; pd += delPD)
			{
				geo::VertGangle const gangle{ gangleFor(pd, meaPair) };
				oss
					<< " pd: " << dat::infoString(pd)
					<< " gangle: " << dat::infoString(gangle)
					<< std::endl;
			}
			return oss.str();
		}

	}; // Interior

	using MeaPair = std::pair<dat::Spot, dat::Spot>;

	//! Spots covering detector size
	std::vector<dat::Spot>
	detSpots
		( dat::Extents const & detSize
		, size_t const & delta = { 100u }
		)
	{
		std::vector<dat::Spot> spots;
		for (size_t row{0u} ; row < detSize.high() ; row += delta)
		{
			for (size_t col{0u} ; col < detSize.wide() ; col += delta)
			{
				dat::Spot const spot{ (double)row, (double)col };
				spots.emplace_back(spot);
			}
		}
		return spots;
	}

	//! Combinatorial pairing of spots
	std::vector<MeaPair>
	detPairs
		( std::vector<dat::Spot> const & spots
		)
	{
		std::vector<MeaPair> pairs;
		pairs.reserve(math::sq(spots.size()));
		for (dat::Spot const & spot1 : spots)
		{
			for (dat::Spot const & spot2 : spots)
			{
				pairs.emplace_back(std::make_pair(spot1, spot2));
			}
		}
		return pairs;
	}

	//! Vector with first two components set to spot coordinates
	inline
	ga::Vector
	detVecFor
		( dat::Spot const & spot
		)
	{
		return ga::Vector{ spot[0], spot[1], 0. };
	}
}

//! Check principal distance calibration
std::string
cam_fit_test1
	()
{
	std::ostringstream oss;

	// simulate a detector
	dat::Extents const detSize{ 2000u, 3000u };
	dat::Spot const rcCenter{ dat::centerOf(detSize) };
	ga::Vector const center{ rcCenter[0], rcCenter[1], 0. };

	double const expPD{ 1000. };

	std::vector<MeaPair> const meaPairs{ detPairs(detSpots(detSize, 123u)) };
	size_t count{ 0u };
	for (MeaPair const & meaPair : meaPairs)
	{
		Interior const inner(detSize); // for test comparisons
		double const tol{ dat::diagonalMag(detSize) * std::sqrt(math::eps) };

		// compute interior angle magnitude for test case
		ga::Vector const vecMea1{ detVecFor(meaPair.first) };
		ga::Vector const vecMea2{ detVecFor(meaPair.second) };
		ga::Vector const vecPD{ center + expPD * ga::e3 };
		geo::VertGangle const imgGangle
			{ vecPD, std::make_pair(vecMea1, vecMea2) };
		double const beta{ imgGangle.angleMag() };

		// fit principal distance to this angle
		std::vector<double> const gotPDs
			{ cam::fit::principalDistanceFor(meaPair, beta, detSize) };

		// if same point, expect no solution
		if (dat::nearlyEquals(meaPair.first, meaPair.second))
		{
			if (! gotPDs.empty())
			{
				oss << "Failure of empty return for zero angle test" << '\n';
			}
		}
		else
		{
			// check that solution(s) reproduce interior angle
			for (double const & gotPD : gotPDs)
			{
				double const expBeta{ inner.beta(expPD, meaPair) };
				double const gotBeta{ inner.beta(gotPD, meaPair) };
				if (! dat::nearlyEquals(gotBeta, expBeta, tol))
				{
					oss << "Failure of angle beta test" << std::endl;
				}
			}

			// check that at least one of the solutions is correct
			bool okay{ false };
			for (double const & gotPD : gotPDs)
			{
				if (dat::nearlyEquals(gotPD, expPD, tol))
				{
					okay = true;
				}
			}
			if (! okay)
			{
				oss << "Failure to recover principal distance test" << '\n';
			}
		}

		// if error encountered, display diagnostic info
		if (! oss.str().empty())
		{
			oss << dat::infoString(expPD, "expPD") << std::endl;
			oss << dat::infoString(vecMea1, "vecMea1") << std::endl;
			oss << dat::infoString(vecMea2, "vecMea2") << std::endl;
			oss << dat::infoString(beta, "beta") << std::endl;

			// display angle info for solution(s)
			double const expBeta{ inner.beta(expPD, meaPair) };
			if (gotPDs.empty())
			{
				oss << "<empty solutions>" << std::endl;
			}
			for (double const & gotPD : gotPDs)
			{
				double const gotBeta{ inner.beta(gotPD, meaPair) };
				double const difBeta{ gotBeta - expBeta };
				double const difPD{ gotPD - expPD };
				oss
					<< "...   expPD: " << io::sprintf("%12.6f", expPD)
					<< "   gotPD: " << io::sprintf("%12.6f", gotPD)
					<< "   difPD: " << io::sprintf("%12.5e", difPD)
					<< "   tol: " << io::sprintf("%12.5e", tol)
					<< std::endl;
				oss
					<< "... expBeta: " << io::sprintf("%12.6f", expBeta)
					<< " gotBeta: " << io::sprintf("%12.6f", gotBeta)
					<< " difBeta: " << io::sprintf("%12.5e", difBeta)
					<< "   tol: " << io::sprintf("%12.5e", tol)
					<< std::endl;
			}

			// display expected profile
			oss << inner.infoProfile(meaPair, 0., 2000., 100.) << std::endl;

			break;
		}
		else
		{
			++count;
		}
	}

	if (count < 100u)
	{
		oss << "Failure to consider enough cases: count = " << count << '\n';
	}
	// io::out() << count << std::endl;

	return oss.str();
}

}

//! Unit test for cam::fit
int
main
	( int const /*argc*/
	, char const * const * /*argv*/
	)
{
	std::ostringstream oss;

	// run tests
	oss << cam_fit_test0();
	oss << cam_fit_test1();

	// check/report results
	std::string const errMessages(oss.str());
	if (! errMessages.empty())
	{
		io::err() << errMessages << std::endl;
		return 1;
	}
	return 0;
}
<|endoftext|>


<commit_before>
#ifdef _WIN32
#include <windows.h>
#else
#include <errno.h>
#include <cstring>
#include <pthread.h>
#endif

#include <nstd/Error.h>
#include <nstd/Map.h>
#include <nstd/Debug.h>

class Error::Private
{
public:
#ifdef _WIN32
  static CRITICAL_SECTION cs;
#else
  static pthread_mutex_t mutex;
#endif
  static Map<uint32_t, String> userErrorStrings;
  static class Framework
  {
  public:
    Framework()
    {
#ifdef _WIN32
      InitializeCriticalSection(&cs);
#else
      pthread_mutexattr_t attr; // TODO: use global var for this?
      pthread_mutexattr_init(&attr);
      VERIFY(pthread_mutex_init(&mutex, &attr) == 0);
#endif
    }
    ~Framework()
    {
#ifdef _WIN32
      DeleteCriticalSection(&cs);
#else
      VERIFY(pthread_mutex_destroy(&mutex) == 0);
#endif
    }
  } framework;
};

#ifdef _WIN32
CRITICAL_SECTION Error::Private::cs;
#else
pthread_mutex_t CRITICAL_SECTION Error::Private::mutex;
#endif
Map<uint32_t, String> Error::Private::userErrorStrings;
Error::Private::Framework Error::Private::framework;

void_t Error::setLastError(uint_t error)
{
#ifdef _WIN32
  SetLastError((DWORD)error);
#else
  errno = (int)error;
#endif
}

uint_t Error::getLastError()
{
#ifdef _WIN32
  return (uint_t)GetLastError();
#else
  return errno;
#endif
}

String Error::getErrorString(uint_t error)
{
  if(error == 0x10000)
  {
    String errorStr;
#ifdef _WIN32
    EnterCriticalSection(&Private::cs);
#else
    VERIFY(pthread_mutex_lock(&Private::mutex) == 0);
#endif

#ifdef _WIN32
    uint32_t threadId = (uint32_t)GetCurrentThreadId();
#else
    uint32_t threadId = (uint32_t)pthread_self();
#endif
    Map<uint32_t, String>::Iterator it = Private::userErrorStrings.find(threadId);
    if(it != Private::userErrorStrings.end())
      errorStr = *it;
#ifdef _WIN32
    LeaveCriticalSection(&Private::cs);
#else
    VERIFY(pthread_mutex_unlock(&Private::mutex) == 0);
#endif
    return errorStr;
  }

#ifdef _WIN32
  TCHAR errorMessage[256];
  DWORD len = FormatMessage(
        FORMAT_MESSAGE_FROM_SYSTEM |
        FORMAT_MESSAGE_IGNORE_INSERTS,
        NULL,
        error,
        MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
        (LPTSTR) errorMessage,
        256, NULL );
  ASSERT(len >= 0 && len <= 256);
  while(len > 0 && String::isSpace(errorMessage[len - 1]))
    --len;
  errorMessage[len] = '\0';
  return String(errorMessage, len);
#else
  const char* errorMessage = strerror(error);
  return String(errorMessage, String::length(errorMessage));
#endif
}

void_t Error::setErrorString(const String& error)
{
#ifdef _WIN32
  EnterCriticalSection(&Private::cs);
#else
  VERIFY(pthread_mutex_lock(&Private::mutex) == 0);
#endif

#ifdef _WIN32
  uint32_t threadId = (uint32_t)GetCurrentThreadId();
#else
  uint32_t threadId = (uint32_t)pthread_self();
#endif

  String* threadErrorMsg = 0;
  for(Map<uint32_t, String>::Iterator i = Private::userErrorStrings.begin(), end = Private::userErrorStrings.end(), next; i != end; i = next)
  {
    next = i;
    ++next;

#ifdef _WIN32
    HANDLE handle = OpenThread(DELETE, FALSE, i.key());
    if(handle == NULL)
    {
      Private::userErrorStrings.remove(i);
      continue;
    }
    else
      CloseHandle(handle);
#else
    int policy;
    struct sched_param param;
    if(pthread_getschedparam((pthread_t)i.key(), &policy, &param) != 0 && errno == ESRCH)
    {
      Private::userErrorStrings.remove(i);
      continue;
    }
#endif
    if(i.key() == threadId)
      threadErrorMsg = &*i;
  }
  if(threadErrorMsg)
    *threadErrorMsg = error;
  else
    Private::userErrorStrings.insert(threadId, error);
  setLastError(0x10000);
#ifdef _WIN32
  LeaveCriticalSection(&Private::cs);
#else
  VERIFY(pthread_mutex_unlock(&Private::mutex) == 0);
#endif
}
<commit_msg>Error: Fixed Linux implementation of setErrorString<commit_after>
#ifdef _WIN32
#include <windows.h>
#else
#include <errno.h>
#include <cstring>
#include <pthread.h>
#include <sched.h>
#include <unistd.h>
#include <sys/syscall.h>
#endif

#include <nstd/Error.h>
#include <nstd/Map.h>
#include <nstd/Debug.h>

class Error::Private
{
public:
#ifdef _WIN32
  static CRITICAL_SECTION cs;
#else
  static pthread_mutex_t mutex;
#endif
  static Map<uint32_t, String> userErrorStrings;
  static class Framework
  {
  public:
    Framework()
    {
#ifdef _WIN32
      InitializeCriticalSection(&cs);
#else
      pthread_mutexattr_t attr; // TODO: use global var for this?
      pthread_mutexattr_init(&attr);
      VERIFY(pthread_mutex_init(&mutex, &attr) == 0);
#endif
    }
    ~Framework()
    {
#ifdef _WIN32
      DeleteCriticalSection(&cs);
#else
      VERIFY(pthread_mutex_destroy(&mutex) == 0);
#endif
    }
  } framework;
};

#ifdef _WIN32
CRITICAL_SECTION Error::Private::cs;
#else
pthread_mutex_t Error::Private::mutex;
#endif
Map<uint32_t, String> Error::Private::userErrorStrings;
Error::Private::Framework Error::Private::framework;

void_t Error::setLastError(uint_t error)
{
#ifdef _WIN32
  SetLastError((DWORD)error);
#else
  errno = (int)error;
#endif
}

uint_t Error::getLastError()
{
#ifdef _WIN32
  return (uint_t)GetLastError();
#else
  return errno;
#endif
}

String Error::getErrorString(uint_t error)
{
  if(error == 0x10000)
  {
    String errorStr;
#ifdef _WIN32
    EnterCriticalSection(&Private::cs);
#else
    VERIFY(pthread_mutex_lock(&Private::mutex) == 0);
#endif

#ifdef _WIN32
    uint32_t threadId = (uint32_t)GetCurrentThreadId();
#else
    uint32_t threadId = (uint32_t)syscall(__NR_gettid);
#endif
    Map<uint32_t, String>::Iterator it = Private::userErrorStrings.find(threadId);
    if(it != Private::userErrorStrings.end())
      errorStr = *it;
#ifdef _WIN32
    LeaveCriticalSection(&Private::cs);
#else
    VERIFY(pthread_mutex_unlock(&Private::mutex) == 0);
#endif
    return errorStr;
  }

#ifdef _WIN32
  TCHAR errorMessage[256];
  DWORD len = FormatMessage(
        FORMAT_MESSAGE_FROM_SYSTEM |
        FORMAT_MESSAGE_IGNORE_INSERTS,
        NULL,
        error,
        MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
        (LPTSTR) errorMessage,
        256, NULL );
  ASSERT(len >= 0 && len <= 256);
  while(len > 0 && String::isSpace(errorMessage[len - 1]))
    --len;
  errorMessage[len] = '\0';
  return String(errorMessage, len);
#else
  const char* errorMessage = strerror(error);
  return String(errorMessage, String::length(errorMessage));
#endif
}

void_t Error::setErrorString(const String& error)
{
#ifdef _WIN32
  EnterCriticalSection(&Private::cs);
#else
  VERIFY(pthread_mutex_lock(&Private::mutex) == 0);
#endif

#ifdef _WIN32
  uint32_t threadId = (uint32_t)GetCurrentThreadId();
#else
  uint32_t threadId = (uint32_t)syscall(__NR_gettid);
#endif

  String* threadErrorMsg = 0;
  for(Map<uint32_t, String>::Iterator i = Private::userErrorStrings.begin(), end = Private::userErrorStrings.end(), next; i != end; i = next)
  {
    next = i;
    ++next;

#ifdef _WIN32
    HANDLE handle = OpenThread(DELETE, FALSE, i.key());
    if(handle == NULL)
    {
      Private::userErrorStrings.remove(i);
      continue;
    }
    else
      CloseHandle(handle);
#else
    cpu_set_t cpuset;
    if(sched_getaffinity((pid_t)i.key(), sizeof(cpu_set_t), &cpuset) != 0)
    {
      Private::userErrorStrings.remove(i);
      continue;
    }
#endif
    if(i.key() == threadId)
      threadErrorMsg = &*i;
  }
  if(threadErrorMsg)
    *threadErrorMsg = error;
  else
    Private::userErrorStrings.insert(threadId, error);
  setLastError(0x10000);
#ifdef _WIN32
  LeaveCriticalSection(&Private::cs);
#else
  VERIFY(pthread_mutex_unlock(&Private::mutex) == 0);
#endif
}
<|endoftext|>