/*
* The stack must have three values in this order:
* 1. x coordinate (a number)
* 2. y coordinate (a number)
* 3. z coordinate (a number)
*/
int LuaSetRelativePosition(lua_State* pt_lua_state) {
/* Check parameters */
if(lua_gettop(pt_lua_state) != 3) {
return luaL_error(pt_lua_state, "robot.quadrotor.set_relative_position() expects 3 arguments");
}
luaL_checktype(pt_lua_state, 1, LUA_TNUMBER);
luaL_checktype(pt_lua_state, 2, LUA_TNUMBER);
luaL_checktype(pt_lua_state, 3, LUA_TNUMBER);
/* Perform action */
CLuaUtility::GetDeviceInstance<CCI_QuadRotorPositionActuator>(pt_lua_state, "quadrotor")->
SetRelativePosition(CVector3(lua_tonumber(pt_lua_state, 1),
lua_tonumber(pt_lua_state, 2),
lua_tonumber(pt_lua_state, 3)));
return 0;
}
bool CRABEquippedEntityGridEntityUpdater::operator()(CRABEquippedEntity& c_entity) {
try {
m_cCellUpdater.SetEntity(c_entity);
m_cGrid.ForCellsInBoxRange(c_entity.GetPosition(),
CVector3(c_entity.GetRange(),
c_entity.GetRange(),
c_entity.GetRange()),
m_cCellUpdater);
/* Continue with the other entities */
return true;
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("While updating the RAB entity grid for RAB entity \"" << c_entity.GetContext() << c_entity.GetId() << "\"", ex);
}
}
//-------------------------------------------------------------
//- CalcNorms
//- Calculate the vertex and face normals
//-------------------------------------------------------------
void CalcNorms()
{
//Calculate face normals first
for(int x = 0; x < 10; x++)
{
CVector3 vTmp = CalcFaceNormal(CVector3(verts[Tris[x][2]]), CVector3(verts[Tris[x][1]]), CVector3(verts[Tris[x][0]]));
memcpy(facenormals[x], vTmp.Get(), 12);
}
//Calculate the vertex normals
for(x = 0; x < 10; x++)
{
int iShared[10]; //Indices of shared faces
int iNumShared = 0; //Number of faces that share vertex
//first find out which faces share the vertex
for(int y = 0; y < 10; y++)
{
for(int z = 0; z < 3; z++)
{
if(Tris[y][z] == x)
{
iShared[iNumShared] = y;
iNumShared ++;
}
}
}
//Calculate a normal by averaging the face normals of the shared faces
CVector3 finalNorm;
for(y = 0; y < iNumShared; y++)
{
finalNorm += facenormals[iShared[y]];
}
finalNorm /= (float)iNumShared;
memcpy(&vertnormals[x], finalNorm.Get(), 12);
}
}
// draw function for adding graphics to a foot-bot
void iAnt_qt_user_functions::DrawFood(CFootBotEntity& entity) {
iAnt_controller& c = dynamic_cast<iAnt_controller&>
(entity.GetControllableEntity().GetController());
UpdateDrawInWorldData(c);
if(c.IsHoldingFood() == true) {
//#ifdef __APPLE__
//Edit here for drawing trails
DrawCylinder(CVector3(0.0f, 0.0f, 0.3f), CQuaternion(), 0.05f, 0.025f, CColor::BLACK);
//#else
// DrawCylinder(0.05f, 0.025f, CVector3(0.0f, 0.0f, 0.3f), CColor::BLACK);
//#endif
}
}
void CEyeBotCircle::TakeOff() {
if(m_eState != STATE_TAKE_OFF) {
/* State initialization */
m_eState = STATE_TAKE_OFF;
m_cCircleCenter = m_pcPosSens->GetReading().Position + CVector3(0.0f, 0.0f, ALTITUDE);
m_cTargetPos = m_cCircleCenter;
m_pcPosAct->SetAbsolutePosition(m_cTargetPos);
}
else {
if(Distance(m_cTargetPos, m_pcPosSens->GetReading().Position) < PROXIMITY_TOLERANCE) {
/* State transition */
LeaveCircleCenter();
}
}
}
_MEMBER_FUNCTION_IMPL(Audio, setPosition)
{
CAudio * pAudio = script_getinstance<CAudio *>(pVM);
float fX, fY, fZ, fR;
script_getfloat ( pVM, 1, &fX );
script_getfloat ( pVM, 2, &fY );
script_getfloat ( pVM, 3, &fZ );
script_getfloat ( pVM, 4, &fR );
pAudio->SetPosition (CVector3(fX, fY, fZ), fR );
script_pushbool(pVM, true);
return 1;
}
// getVehicleAngularVelocity(vehicleid)
CVector3 CVehicleModuleNatives::GetAngularVelocity(EntityId vehicleid)
{
if(g_pVehicleManager->DoesExist(vehicleid))
{
CVehicle * pVehicle = g_pVehicleManager->GetAt(vehicleid);
if(pVehicle)
{
CVector3 vecTurnSpeed;
pVehicle->GetTurnSpeed(vecTurnSpeed);
return vecTurnSpeed;
}
}
return CVector3();
}
// getVehicleCoordinates(vehicleid)
CVector3 CVehicleModuleNatives::GetCoordinates(EntityId vehicleid)
{
if(g_pVehicleManager->DoesExist(vehicleid))
{
CVehicle * pVehicle = g_pVehicleManager->GetAt(vehicleid);
if(pVehicle)
{
CVector3 vecPosition;
pVehicle->GetPosition(vecPosition);
return vecPosition;
}
}
return CVector3();
}
// getVehicleRotation(vehicleid)
CVector3 CVehicleModuleNatives::GetRotation(EntityId vehicleId)
{
if(g_pVehicleManager->DoesExist(vehicleId))
{
CVehicle * pVehicle = g_pVehicleManager->GetAt(vehicleId);
if(pVehicle)
{
CVector3 vecRotation;
pVehicle->GetRotation(vecRotation);
return vecRotation;
}
}
return CVector3();
}
CNetworkEntity::CNetworkEntity(eEntityType eType)
: m_vecPosition(CVector3()),
m_vecRotation(CVector3()),
m_vecMoveSpeed(CVector3()),
m_vecTurnSpeed(CVector3()),
m_vecDirection(CVector3()),
m_vecRoll(CVector3()),
m_entityId(INVALID_ENTITY),
m_eType(eType)
{
}
CNetworkEntity::CNetworkEntity(eEntityType eType, EntityId entityId)
: m_vecPosition(CVector3()),
m_vecRotation(CVector3()),
m_vecMoveSpeed(CVector3()),
m_vecTurnSpeed(CVector3()),
m_vecDirection(CVector3()),
m_vecRoll(CVector3()),
m_entityId(entityId),
m_eType(eType)
{
}
CLightEntity& CLoopFunctions::AddLight(const std::string& str_id,
const CVector3& c_position,
Real f_intensity,
const CColor& c_color) {
/* Build the XML tree */
TConfigurationNode tRootNode("light");
SetNodeAttribute(tRootNode, "id", str_id);
SetNodeAttribute(tRootNode, "position", c_position);
SetNodeAttribute(tRootNode, "orientation", CVector3());
SetNodeAttribute(tRootNode, "intensity", f_intensity);
SetNodeAttribute(tRootNode, "color", c_color);
/* Add the entity to the space */
CLightEntity& cEntity = AddEntityToSpace<CLightEntity>(tRootNode);
/* Return the entity */
return cEntity;
}
CVector3 CSplineInterpolatorVector3::intepolate(S32 index, F32 t){
if ((index + 1) == nodes.size()){
return nodes[index];
}
if (t == 0.0f){
return nodes[index];
}else if(t == 1.0f){
return nodes[index + 1];
}
F32 t2, t3;
t2 = t * t;
t3 = t2 * t;
CVector4 powers(t3, t2, t, 1);
// ret = powers * mCoeffs * (point1, point2, tangent1, tangent2)
CVector3& point1 = nodes[index];
CVector3& point2 = nodes[index+1];
CVector3& tan1 = mTangents[index];
CVector3& tan2 = mTangents[index+1];
CMatrix4 mpt;
FMatrix4 &pt = mpt.m;
pt[0][0] = point1.x;
pt[0][1] = point1.y;
pt[0][2] = point1.z;
pt[0][3] = 1.0f;
pt[1][0] = point2.x;
pt[1][1] = point2.y;
pt[1][2] = point2.z;
pt[1][3] = 1.0f;
pt[2][0] = tan1.x;
pt[2][1] = tan1.y;
pt[2][2] = tan1.z;
pt[2][3] = 1.0f;
pt[3][0] = tan2.x;
pt[3][1] = tan2.y;
pt[3][2] = tan2.z;
pt[3][3] = 1.0f;
CVector4 ret = mCoeffs * mpt * powers;
return CVector3(ret.x, ret.y, ret.z);
}
void CWorldEditor::CreateBaseAxis()
{
auto sceneRoot = m_overlayViewport->GetSceneRoot();
static const CVector3 g_arrowScale(0.075f, 0.25f, 0.075f);
{
auto baseAxisNode = Palleon::CSceneNode::Create();
baseAxisNode->SetPosition(CVector3(289.2f, 5.00f, -563.f));
sceneRoot->AppendChild(baseAxisNode);
{
auto axisMesh = Palleon::CAxisMesh::Create();
axisMesh->SetScale(CVector3(1, 1, 1));
baseAxisNode->AppendChild(axisMesh);
}
//X arrow
{
auto coneMesh = Palleon::CConeMesh::Create();
coneMesh->SetPosition(CVector3(1, 0, 0));
coneMesh->SetRotation(CQuaternion(CVector3(0, 0, 1), M_PI / 2.f));
coneMesh->SetScale(g_arrowScale);
coneMesh->GetMaterial()->SetColor(CColor(1, 0, 0, 1));
baseAxisNode->AppendChild(coneMesh);
}
//Y arrow
{
auto coneMesh = Palleon::CConeMesh::Create();
coneMesh->SetPosition(CVector3(0, 1, 0));
coneMesh->SetScale(g_arrowScale);
coneMesh->GetMaterial()->SetColor(CColor(0, 1, 0, 1));
baseAxisNode->AppendChild(coneMesh);
}
//Z arrow
{
auto coneMesh = Palleon::CConeMesh::Create();
coneMesh->SetPosition(CVector3(0, 0, 1));
coneMesh->SetRotation(CQuaternion(CVector3(1, 0, 0), -M_PI / 2.f));
coneMesh->SetScale(g_arrowScale);
coneMesh->GetMaterial()->SetColor(CColor(0, 0, 1, 1));
baseAxisNode->AppendChild(coneMesh);
}
}
}
// Ammo constructor intialises ammo-specific data and passes its parameters to the base
// class constructor
CAmmoEntity::CAmmoEntity
(
CEntityTemplate* entityTemplate,
TEntityUID UID,
const TInt32& refillSize,
const string& name /*= ""*/,
const CVector3& position /*= CVector3::kOrigin*/,
const CVector3& rotation /*= CVector3(0.0f, 0.0f, 0.0f)*/,
const CVector3& scale /*= CVector3(1.0f, 1.0f, 1.0f)*/
) : CEntity( entityTemplate, UID, name, position + CVector3(0.0f, 100.0f, 0.0f), rotation, scale )
{
m_RefillSize = refillSize;
m_Height = position.y;
m_FallSpeed = 20.0f;
landed = false;
m_SinWave = 0.0f;
}
void CIVTrain::CreateWagons(BYTE iWagons)
{
// Check if we're a wagon
if(m_eTrainType == TYPE_WAGON)
return;
int iWagonCount = 0;
while(iWagonCount < iWagons)
{
CVehicleEntity * pVehicle = new CVehicleEntity(127, CVector3(), 0.0f, 0, 0, 0, 0);
m_pWagonEntity[iWagonCount] = new CIVTrain;
m_pWagonEntity[iWagonCount]->m_eTrainType = TYPE_WAGON;
m_pWagonEntity[iWagonCount]->m_pVehicle = pVehicle;
m_pWagonEntity[iWagonCount]->m_pVehicle->SetId(g_pCore->GetGame()->GetVehicleManager()->FindFreeSlot());
m_pWagonEntity[iWagonCount]->m_pVehicle->Create();
g_pCore->GetGame()->GetVehicleManager()->Add(m_pWagonEntity[iWagonCount]->m_pVehicle);
iWagonCount++;
}
}
void CVehicleEntity::UpdateTargetPosition()
{
// Do we have a target position?
if(HasTargetPosition())
{
// Get our position
CVector3 vecCurrentPosition;
GetPosition(vecCurrentPosition);
// Get the factor of time spent from the interpolation start
// to the current time.
unsigned long ulCurrentTime = SharedUtility::GetTime();
float fAlpha = Math::Unlerp(m_interp.pos.ulStartTime, ulCurrentTime, m_interp.pos.ulFinishTime);
// Don't let it overcompensate the error too much
fAlpha = Math::Clamp(0.0f, fAlpha, 1.5f);
// Get the current error portion to compensate
float fCurrentAlpha = (fAlpha - m_interp.pos.fLastAlpha);
m_interp.pos.fLastAlpha = fAlpha;
// Apply the error compensation
CVector3 vecCompensation = Math::Lerp(CVector3(), fCurrentAlpha, m_interp.pos.vecError);
// If we finished compensating the error, finish it for the next pulse
if(fAlpha == 1.5f)
m_interp.pos.ulFinishTime = 0;
// Calculate the new position
CVector3 vecNewPosition = (vecCurrentPosition + vecCompensation);
if (!((vecCurrentPosition - m_interp.pos.vecTarget).Length() <= 3.0 /* maybe adjust this value a bit if we need earlier correction */))
{
// Abort position interpolation
m_interp.pos.ulFinishTime = 0;
vecNewPosition = m_interp.pos.vecTarget;
}
// Set our new position
SetPosition(vecNewPosition, true, false);
}
}
CVector3 CQuake3BSP::TraceBox(CVector3 vStart, CVector3 vEnd, CVector3 vMin, CVector3 vMax)
{
m_traceType = TYPE_BOX; // Set the trace type to a BOX
m_vTraceMaxs = vMax; // Set the max value of our AABB
m_vTraceMins = vMin; // Set the min value of our AABB
m_bCollided = false; // Reset the collised flag
/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *
// Here we initialize our variables for a new round of collision checks
m_bTryStep = false;
m_bGrounded = false;
/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *
// Grab the extend of our box (the largest size for each x, y, z axis)
m_vExtents = CVector3(-m_vTraceMins.x > m_vTraceMaxs.x ? -m_vTraceMins.x : m_vTraceMaxs.x,
-m_vTraceMins.y > m_vTraceMaxs.y ? -m_vTraceMins.y : m_vTraceMaxs.y,
-m_vTraceMins.z > m_vTraceMaxs.z ? -m_vTraceMins.z : m_vTraceMaxs.z);
// Check if our movement collided with anything, then get back our new position
CVector3 vNewPosition = Trace(vStart, vEnd);
/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *
// Let's check to see if we collided with something and we should try to step up
if(m_bCollided && m_bTryStep)
{
// Try and step up what we collided with
vNewPosition = TryToStep(vNewPosition, vEnd);
}
/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *
// Return our new position
return vNewPosition;
}
void COctree::InitOctree()
{
// Set the subdivided flag to false
m_bSubDivided = false;
// Set the dimensions of the box to false
m_Width = 0;
// Initialize the triangle count
m_TriangleCount = 0;
// Initialize the center of the box to the 0
m_vCenter = CVector3(0, 0, 0);
// Set the triangle list to NULL
m_pVertices = NULL;
// Set the sub nodes to NULL
memset(m_pOctreeNodes, 0, sizeof(m_pOctreeNodes));
}
CEmbodiedEntity::CEmbodiedEntity(CComposableEntity* pc_parent,
const std::string& str_id,
const CVector3& c_position,
const CQuaternion& c_orientation,
bool b_movable) :
CEntity(pc_parent, str_id),
m_bMovable(b_movable),
m_sBoundingBox(NULL),
m_psOriginAnchor(new SAnchor("origin",
0,
CVector3(),
CQuaternion(),
c_position,
c_orientation)),
m_cInitOriginPosition(c_position),
m_cInitOriginOrientation(c_orientation) {
/* Add anchor to map and enable it */
m_mapAnchors[m_psOriginAnchor->Id] = m_psOriginAnchor;
EnableAnchor("origin");
}
bool CEntity::CheckSphereAgainstOOBB( const CVector3& Point, const float& Radius )
{
float distX, distZ;
// First calculate the distance from the X axis
CVector3 v( Matrix().Position(), Point );
distX = abs( Dot( v, Normalise( Matrix().XAxis() ) ) );
// Then calculate the distance from the Z axis
v = CVector3( Matrix().Position(), Point );
distZ = abs( Dot( v, Normalise( Matrix().ZAxis() ) ) );
if( distX <= ( m_XAxisRadius + Radius ) && distZ <= ( m_ZAxisRadius + Radius ) )
{
return true;
}
else
{
return false;
}
}
/*****
* Initialize the controller via the XML configuration file. ARGoS typically
* wants objects & variables initialized here instead of in the constructor(s).
*****/
void iAnt_controller::Init(TConfigurationNode& node) {
/* Shorter names, please. #This_Is_Not_Java */
typedef CCI_PositioningSensor CCI_PS;
typedef CCI_DifferentialSteeringActuator CCI_DSA;
typedef CCI_FootBotProximitySensor CCI_FBPS;
/* Initialize the robot's actuator and sensor objects. */
motorActuator = GetActuator<CCI_DSA>("differential_steering");
compass = GetSensor<CCI_PS> ("positioning");
proximitySensor = GetSensor<CCI_FBPS> ("footbot_proximity");
TConfigurationNode iAnt_params = GetNode(node, "iAnt_params");
GetNodeAttribute(iAnt_params, "RobotForwardSpeed", RobotForwardSpeed);
GetNodeAttribute(iAnt_params, "RobotTurningSpeed", RobotTurningSpeed);
GetNodeAttribute(iAnt_params, "AngleToleranceInDegrees", angleInDegrees);
AngleToleranceInRadians.Set(-ToRadians(angleInDegrees),ToRadians(angleInDegrees));
stepSize = 0.1; /* Assigns the robot's stepSize */
startPosition = CVector3(0.0, 0.0, 0.0);
}
bool CEntity::CheckOOBBtoOOBBIntersection( const float& ZAxisRadius, const float& XAxisRadius,
const CVector3& Position )
{
// First calculate the distance from the X axis
CVector3 v( Matrix().Position(), Position );
float distX = abs( Dot( v, Normalise( Matrix().XAxis() ) ) );
// Then calculate the distance from the Z axis
v = CVector3( Matrix().Position(), Position );
float distZ = abs( Dot( v, Normalise( Matrix().ZAxis() ) ) );
// Now check for an intersection
if( distX <= ( (m_XAxisRadius*0.8) + (XAxisRadius*0.8) ) &&
distZ <= ( (m_ZAxisRadius*0.8) + (ZAxisRadius*0.8) ) )
{
return true;
}
else
{
return false;
}
}
//--------------------------------------------------------------------------------------------------------------------
void RDX11RenderHelper::RenderBox(XMMATRIX& mtWorld, CVector3& min, CVector3& max, DWORD color)
{
SetWorldTM(mtWorld);
BOX_MAKE_PARAM param;
param.min = min;
param.max = max;
param.offset = CVector3(0, 0, 0);
CVertexPC* pVertices = NULL;
CGEOMETRY_CONSTRUCTOR::CreateBoxLine( param, &pVertices);
for (int i =0 ; i< 24; i++)
{
pVertices[i].color = color;
m_LineVertices.Add(pVertices[i]);
}
SAFE_DELETE_ARRAY(pVertices);
GLOBAL::ShaderMgr()->Begin(SHADER_COLOR_VS, SHADER_COLOR_PS);
GLOBAL::RenderStateMgr()->SetDepthStancil(DEPTH_ON_STENCIL_OFF);
DrawLine();
}
// setup any theme related resources
void ThemeDesert::SetupTheme(void)
{
// Desert Theme Specific Settings
m_sThemeName = "Desert";
// still let the base class do default setup
ThemeModel::SetupTheme();
//
m_vFloorColor = CVector3(183, 140, 105);
// setup sounds
//////////////////////////////////////////////////////////////////////////
// SokobanSoundManager &SoundCache = SokobanSoundManager::Instance();
/* string SoundStepFileName = GetFullAssetDirectory();
SoundStepFileName += "audio\\move_forward.au";*/
// SoundCache.LoadSampleFromFile(MOVEEVENT_STEP, ".\\audio\\sample.wav");
}
void CWorldEditor::CreateMap(uint32 mapId)
{
auto sceneRoot = m_mainViewport->GetSceneRoot();
//Create skybox
{
auto skyTexture = Palleon::CTextureLoader::CreateCubeTextureFromFile("./data/global/skybox.dds");
auto skyBox = Palleon::CCubeMesh::Create();
skyBox->SetIsPeggedToOrigin(true);
skyBox->SetScale(CVector3(50, 50, 50));
skyBox->GetMaterial()->SetCullingMode(Palleon::CULLING_CCW);
skyBox->GetMaterial()->SetTexture(0, skyTexture);
skyBox->GetMaterial()->SetTextureCoordSource(0, Palleon::TEXTURE_COORD_CUBE_POS);
sceneRoot->AppendChild(skyBox);
}
{
auto mapLayoutPath = CFileManager::GetResourcePath(mapId);
auto mapLayout = std::make_shared<CMapLayout>();
auto mapStream = Framework::CreateInputStdStream(mapLayoutPath.native());
mapLayout->Read(mapStream);
auto map = std::make_shared<CUmbralMap>(mapLayout);
sceneRoot->AppendChild(map);
}
#if 0
{
auto mapLayoutPath = CFileManager::GetResourcePath(0x29D90002);
auto mapLayout = std::make_shared<CMapLayout>();
mapLayout->Read(Framework::CreateInputStdStream(mapLayoutPath.native()));
auto map = std::make_shared<CUmbralMap>(mapLayout);
sceneRoot->AppendChild(map);
}
#endif
}
TriangleMan::TriangleMan( float radius, const CVector3& center )
{
mRadius = radius;
mPathRadius = mRadius + 10.0f;
mBallRadius = 2.0f;
mCenter = center;
mSpinType = -1;
mPrevSpin = -1;
mState = 0;
mCurTime = 0.0f;
mPrevTime = 0.0f;
mBallCurTime = 0.0f;
mBallPrevTime = 0.0f;
float halfSide = 3.0f * mRadius / (2 * sqrt( 3.0f ));
float littleRadius = sqrt( mRadius*mRadius - halfSide*halfSide );
//top.
mPoints[0] = CVector3( mCenter.x, mCenter.y + mRadius, 0.0f );
//left
mPoints[1] = CVector3( mCenter.x - halfSide, mCenter.y - littleRadius, 0.0f);
//right
mPoints[2] = CVector3( mCenter.x + halfSide, mCenter.y - littleRadius, 0.0f );
mPathColor = CVector3( 0.0f, 0.0f, 0.0f );
mSpreadPointOne = CVector3( 0.0f, 0.0f, 0.0f );
mSpreadPointTwo = CVector3( 0.0f, 0.0f, 0.0f );
//mBodyRef = NULL;
//mRenderComp = NULL;
mRenderComponents.clear();
mRenderComponents.resize(0);
mCurRegion = 0;
}
CVector3 Image3D::GetPixelVectorLaplacian(const CVector3& index)
{
IndexType ind = {static_cast<itk::IndexValueType>(index[0]),static_cast<itk::IndexValueType>(index[1]),static_cast<itk::IndexValueType>(index[2])};
PixelType pixel = laplacianImage_->GetPixel(ind);
return CVector3(pixel[0],pixel[1],pixel[2]);
}
CVector3 Image3D::GetContinuousPixelVector(const CVector3& index)
{
ContinuousIndexType ind; ind[0] = index[0]; ind[1] = index[1]; ind[2] = index[2];
PixelType pixel = vectorImageInterpolator->EvaluateAtContinuousIndex(ind);
return CVector3(pixel[0],pixel[1],pixel[2]);
}
bool CollisionTester::FrustumCircleXZTest( const Frustum& frus, const Circle& cirXZ )
{
Sphere sph( CVector3( cirXZ.mCenter.GetX(), frus.mCenter.GetY(), cirXZ.mCenter.GetY() ), cirXZ.mRadius );
return FrustumSphereTest( frus, sph );
}