static inline double
EvaluateImpl(const LRSDPFunction<SDPType>& function,
const arma::mat& coordinates,
const arma::vec& lambda,
const double sigma)
{
// We can calculate the entire objective in a smart way.
// L(R, y, s) = Tr(C * (R R^T)) -
// sum_{i = 1}^{m} (y_i (Tr(A_i * (R R^T)) - b_i)) +
// (sigma / 2) * sum_{i = 1}^{m} (Tr(A_i * (R R^T)) - b_i)^2
// Let's start with the objective: Tr(C * (R R^T)).
// Simple, possibly slow solution-- see below for optimization opportunity
//
// TODO: Note that Tr(C^T * (R R^T)) = Tr( (CR)^T * R ), so
// multiplying C*R first, and then taking the trace dot should be more memory
// efficient
//
// Similarly for the constraints, taking A*R first should be more efficient
const arma::mat rrt = coordinates * trans(coordinates);
double objective = accu(function.SDP().C() % rrt);
// Now each constraint.
UpdateObjective(objective, rrt, function.SDP().SparseA(), function.SDP().SparseB(),
lambda, 0, sigma);
UpdateObjective(objective, rrt, function.SDP().DenseA(), function.SDP().DenseB(), lambda,
function.SDP().NumSparseConstraints(), sigma);
return objective;
}
void
FltDlg::Show()
{
if (shown) return;
FormWindow::Show();
UpdateSelection();
UpdateObjective();
}
void GStartSinglePlayerWindow::AddGameObjective(EObjectiveType::Enum in_eObjectiveType,
UINT32 in_fTimeLimitYears,
REAL32 in_fValueToReach)
{
GGameObjective l_Obj;
l_Obj.m_eObjectiveType = in_eObjectiveType;
l_Obj.m_fValueToReach = in_fValueToReach;
g_ClientDCL.m_vGameObjectives.push_back(l_Obj);
UpdateObjective(in_eObjectiveType);
}
void
FltDlg::ExecFrame()
{
if (!ship || !ship->GetHangar()) {
manager->HideFltDlg();
}
else {
UpdateSelection();
UpdateObjective();
}
}
GUI::EEventStatus::Enum GStartSinglePlayerWindow::OnCustomEvent(UINT32 in_iEventID, const GUI::GEventData& in_EventData, GUI::GBaseObject * in_pCaller)
{
switch (in_iEventID)
{
case c_iOnPartySelection:
{
INT32 l_iPartyID = (INT32)in_EventData.Data;
for(UINT32 i=0; i<m_vParties.size(); i++)
{
if(m_vParties[i].Id() == l_iPartyID)
{
m_iSelectedPartyID = m_vParties[i].Id();
m_pObjPartyName->Text( g_ClientDAL.GetString( m_vParties[i].NameId() ) );
break;
}
}
}
break;
case c_iOnCountryProfileWindowSelection:
{
INT32 l_iCountryID = (INT32)in_EventData.Data;
if(l_iCountryID > 0)
{
m_pObjCountryCbo->Selected_Content( g_ClientDAL.Country(l_iCountryID).Name() );
SDK::GGameEventSPtr l_Event = CREATE_GAME_EVENT(SP2::Event::GGetCountryParties);
SP2::Event::GGetCountryParties* l_Evt = (SP2::Event::GGetCountryParties*)l_Event.get();
l_Evt->m_iSource = g_Joshua.Client()->Id();
l_Evt->m_iCountryId = g_ClientDAL.Country( m_pObjCountryCbo->Selected_Content() ).Id();
l_Evt->m_iTarget = SDK::Event::ESpecialTargets::Server;
g_Joshua.RaiseEvent(l_Event);
m_pObjStartGameBtn->Enabled(true);
m_pObjPoliticalPartyBtn->Enabled(true);
}
}
break;
//If a combo box was selected
case ECustomEvents::OnComboBoxBeforeOpen:
{
if(in_EventData.Data == m_pObjCountryCbo)
g_ClientDDL.ComboboxSetCountriesAndFlag(g_ClientDAL.Countries(), (GUI::GComboBox*)in_EventData.Data, true);
return GUI::EEventStatus::Handled;
}
case ECustomEvents::OnComboBoxSelectionEvent:
{
//Determine which combo box was selected and update the objective for that box
GUI::GComboBox * l_pCombo = (GUI::GComboBox *)in_EventData.Data;
gassert(l_pCombo, "GStartSinglePlayerWindow::OnCustomEvent Combo box pointer shouldnt be null on custom event");
if(l_pCombo == m_pObjConquerWorldCbo)
{
UpdateObjective(EObjectiveType::ConquerTheWorld);
}
else if(l_pCombo == m_pObjRaiseGDPCbo)
{
UpdateObjective(EObjectiveType::RaiseGDPPerCapita);
}
else if(l_pCombo == m_pObjWorldPeaceCbo)
{
UpdateObjective(EObjectiveType::AchieveWorldPeace);
}
else if(l_pCombo == m_pObjBalanceResCbo)
{
UpdateObjective(EObjectiveType::MeetResourcesNeeds);
}
else if(l_pCombo == m_pObjDevelopNationCbo)
{
UpdateObjective(EObjectiveType::DevelopNation);
}
else if(l_pCombo == m_pObjCountryCbo)
{
if( m_pObjCountryCbo->Selected_Content() != g_ClientDAL.GetString(EStrId::SelectACountry))
{
SDK::GGameEventSPtr l_Event = CREATE_GAME_EVENT(SP2::Event::GGetCountryParties);
SP2::Event::GGetCountryParties* l_Evt = (SP2::Event::GGetCountryParties*)l_Event.get();
l_Evt->m_iSource = g_Joshua.Client()->Id();
l_Evt->m_iCountryId = g_ClientDAL.Country( m_pObjCountryCbo->Selected_Content() ).Id();
l_Evt->m_iTarget = SDK::Event::ESpecialTargets::Server;
g_Joshua.RaiseEvent(l_Event);
m_pObjStartGameBtn->Enabled(true);
m_pObjPoliticalPartyBtn->Enabled(true);
}
}
break;
}
default:
break;
}
return EEventStatus::Handled;
}
/*virtual*/ void CSpammerEnemy::Update(float fDt) /*override*/
{
if (m_fCurrentHealth < 0)
{
Disable();
return;
}
m_pSppitlePistol->UpdateEnemyW(fDt, this);
DirectX::XMFLOAT3 position = GetPosition();
(m_tCollider)->vCenter = DirectX::XMFLOAT3(position.x, position.y, position.z);
m_pLightParams->Position = GetPosition();
m_pLightParams->Position.y += 50;
UpdateObjective(fDt);
CBaseEnemy::Update(fDt);
// Animate
if (m_pAnimation)
{
GetAnimStateMachine().Update(fDt);
GetAnimationBlender().Process(fDt);
m_cDeformer.Process(GetAnimationBlender().GetPose());
if (m_pAnimation->m_pModel->m_pVertexBuffer == NULL)
{
CGameplayState::GetAssetManager()->CreateBuffer(CRenderer::GetReference().GetDevice(), m_cDeformer.GetSkinVerts(),
D3D11_BIND_VERTEX_BUFFER,
&m_pAnimation->m_pModel->m_pVertexBuffer);
}
}
if (m_fCurrentHealth > 0)
{
//Play idle sound, keep them out of sync
if (m_fIdleDelay == 0)
{
//AudioSystemWwise::Get()->PostEvent(AK::EVENTS::PLAY_3DL_HUNTER_IDLE, this);
m_fIdleDelay = -1;
}
//Play steps
float fSpeed = MathHelper::Length(m_vVelocity);
//Count down to the next step
if (m_fStepTimer > 0)
{
m_fStepTimer -= fSpeed * fDt * 0.001f;
if (m_fStepTimer < 0)
{
m_fStepTimer = 0;
}
}
//Play step sounds
else if (fSpeed > 0)
{
AudioSystemWwise::Get()->PostEvent(AK::EVENTS::PLAY_3D_HUNTER_STEP, this);
m_fStepTimer = STEP_DELAY;
}
}
}
void HumanoidController::HumanoidControl()
{
// Perform Optimization
{
UpdateConstraintMatrix();
int maxPriorityLevels = OptimizationSchedule.maxCoeff();
const int numTasks = OptimizationSchedule.size();
if (maxPriorityLevels > 0)
{
for (int level=1; level<=maxPriorityLevels; level++)
{
taskConstrActive.setZero(numTasks);
taskOptimActive.setZero(numTasks);
bool runOpt = false;
for (int i=0; i<numTasks;i ++)
{
if (OptimizationSchedule(i) == level)
{
taskOptimActive(i) = 1;
runOpt = true;
}
else if ((OptimizationSchedule(i) < level) && (OptimizationSchedule(i) > -1))
{
taskConstrActive(i) = 1;
}
}
if (runOpt)
{
UpdateObjective();
UpdateHPTConstraintBounds();
//cout << "Optimizing level " << level << endl;
Optimize();
for (int i=0; i<numTasks;i ++)
{
if (OptimizationSchedule(i) == level)
{
TaskBias(i) += TaskError(i);
//cout << "Optimization Level " << level << " task error " << i << " = " << TaskError(i) << endl;
}
}
}
}
}
/// Compute optimal quantities
// desired change of centroidal momentum
hDotOpt = CentMomMat*qdd + cmBias;
// Desired ZMP info
zmpWrenchOpt.setZero();
Vector6F icsForce, localForce;
Float * nICS = icsForce.data(); // ICS
Float * fICS = nICS+3;
Float * nLoc = localForce.data(); // Local
Float * fLoc = nLoc+3;
for (int k1 = 0; k1 < NS; k1++)
{
LinkInfoStruct * listruct = artic->m_link_list[SupportIndices[k1]];
CartesianVector tmp;
localForce = SupportXforms[k1].transpose()*fs.segment(6*k1,6);
// Apply Spatial Force Transform Efficiently
// Rotate Quantities
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,nLoc,nICS);
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,fLoc,fICS);
// Add the r cross f
CROSS_PRODUCT(listruct->link_val2.p_ICS,fICS,tmp);
ADD_CARTESIAN_VECTOR(nICS,tmp);
zmpWrenchOpt+=icsForce;
}
transformToZMP(zmpWrenchOpt,zmpPosOpt);
// Form Joint Input and simulate
VectorXF jointInput = VectorXF::Zero(STATE_SIZE);
int k = 7;
// Skip over floating base (i=1 initially)
for (int i=1; i<artic->m_link_list.size(); i++)
{
LinkInfoStruct * linki = artic->m_link_list[i];
if (linki->dof)
{
//cout << "Link " << i << " dof = " << linki->dof << endl;
//cout << "qd = " << qdDm.segment(k,linki->dof).transpose() << endl;
jointInput.segment(k,linki->dof) = tau.segment(linki->index_ext-6,linki->dof);
k+=linki->link->getNumDOFs();
}
}
//cout << "Tau = " << tau.transpose() << endl;
//cout << "Joint input = " << jointInput.transpose() << endl;
//exit(-1);
//jointInput.segment(7,NJ) = tau;
artic->setJointInput(jointInput.data());
/// verification
ComputeActualQdd(qddA);
}
#ifdef CONTROL_DEBUG
// Debug Code
{
}
#endif
}
void HumanoidController::HumanoidControl(ControlInfo & ci) {
int taskRow = 0;
Float discountFactor = 1;
dmTimespec tv1, tv2, tv3, tv4;
//Update Graphics Variables
{
ComPos[0] = pCom(0);
ComPos[1] = pCom(1);
ComPos[2] = pCom(2);
ComDes[0] = pComDes(0);
ComDes[1] = pComDes(1);
ComDes[2] = pComDes(2);
}
// Perform Optimization
{
dmGetSysTime(&tv2);
UpdateConstraintMatrix();
int maxPriorityLevels = OptimizationSchedule.maxCoeff();
const int numTasks = OptimizationSchedule.size();
if (maxPriorityLevels > 0) {
for (int level=1; level<=maxPriorityLevels; level++) {
taskConstrActive.setZero(numTasks);
taskOptimActive.setZero(numTasks);
bool runOpt = false;
for (int i=0; i<numTasks;i ++) {
if (OptimizationSchedule(i) == level) {
taskOptimActive(i) = 1;
runOpt = true;
}
else if ((OptimizationSchedule(i) < level) && (OptimizationSchedule(i) > -1)) {
taskConstrActive(i) = 1;
}
}
if (runOpt) {
UpdateObjective();
UpdateHPTConstraintBounds();
dmGetSysTime(&tv3);
//cout << "Optimizing level " << level << endl;
Optimize();
for (int i=0; i<numTasks;i ++) {
if (OptimizationSchedule(i) == level) {
TaskBias(i) += TaskError(i);
//cout << "Optimization Level " << level << " task error " << i << " = " << TaskError(i) << endl;
}
}
}
}
}
else {
UpdateObjective();
UpdateHPTConstraintBounds();
dmGetSysTime(&tv3);
Optimize();
}
//exit(-1);
// Extract Results
hDotOpt = CentMomMat*qdd + cmBias;
// Form Joint Input and simulate
VectorXF jointInput = VectorXF::Zero(STATE_SIZE);
// Extact Desired ZMP info
zmpWrenchOpt.setZero();
Vector6F icsForce, localForce;
Float * nICS = icsForce.data(), * nLoc = localForce.data();
Float * fICS = nICS+3, * fLoc = nLoc+3;
for (int k1 = 0; k1 < NS; k1++) {
LinkInfoStruct * listruct = artic->m_link_list[SupportIndices[k1]];
CartesianVector tmp;
localForce = SupportXforms[k1].transpose()*fs.segment(6*k1,6);
// Apply Spatial Force Transform Efficiently
// Rotate Quantities
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,nLoc,nICS);
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,fLoc,fICS);
// Add the r cross f
CROSS_PRODUCT(listruct->link_val2.p_ICS,fICS,tmp);
ADD_CARTESIAN_VECTOR(nICS,tmp);
zmpWrenchOpt+=icsForce;
}
transformToZMP(zmpWrenchOpt,zmpPosOpt);
int k = 7;
// Skip over floating base (i=1 initially)
for (int i=1; i<artic->m_link_list.size(); i++) {
LinkInfoStruct * linki = artic->m_link_list[i];
if (linki->dof) {
//cout << "Link " << i << " dof = " << linki->dof << endl;
//cout << "qd = " << qdDm.segment(k,linki->dof).transpose() << endl;
jointInput.segment(k,linki->dof) = tau.segment(linki->index_ext-6,linki->dof);
k+=linki->link->getNumDOFs();
}
}
//cout << "Tau = " << tau.transpose() << endl;
//cout << "Joint input = " << jointInput.transpose() << endl;
//exit(-1);
//jointInput.segment(7,NJ) = tau;
artic->setJointInput(jointInput.data());
ComputeActualQdd(qddA);
dmGetSysTime(&tv4);
}
//Populate Control Info Struct
{
ci.calcTime = timeDiff(tv1,tv2);
ci.setupTime = timeDiff(tv2,tv3);
ci.optimTime = timeDiff(tv3,tv4);
ci.totalTime = timeDiff(tv1,tv4);
ci.iter = iter;
}
#ifdef CONTROL_DEBUG
// Debug Code
{
cout << "q " << q.transpose() << endl;
cout << "qd " << qdDm.transpose() << endl;
cout << "qd2 " << qd.transpose() << endl;
cout << "Task Bias " << TaskBias << endl;
//cout << "H = " << endl << artic->H << endl;
cout << "CandG = " << endl << artic->CandG.transpose() << endl;
if (simThread->sim_time > 0) {
cout << setprecision(5);
MSKboundkeye key;
double bl,bu;
for (int i=0; i<numCon; i++) {
MSK_getbound(task, MSK_ACC_VAR, i, &key, &bl, &bu);
cout << "i = " << i;
switch (key) {
case MSK_BK_FR:
cout << " Free " << endl;
break;
case MSK_BK_LO:
cout << " Lower Bound " << endl;
break;
case MSK_BK_UP:
cout << " Upper Bound " << endl;
break;
case MSK_BK_FX:
cout << " Fixed " << endl;
break;
case MSK_BK_RA:
cout << " Ranged " << endl;
break;
default:
cout << " Not sure(" << key << ")" << endl;
break;
}
cout << bl << " to " << bu << endl;
}
cout << "x(57) = " << xx(57) << endl;
cout << "tau = " << tau.transpose() << endl;
cout << "qdd = " << qdd.transpose() << endl;
cout << "fs = " << fs.transpose() << endl;
//cout << "lambda = " << lambda.transpose() << endl;
VectorXF a = TaskJacobian * qdd;
//cout << "a" << endl;
VectorXF e = TaskJacobian * qdd - TaskBias;
cout << "e = " << e.transpose() << endl;
MatrixXF H = artic->H;
VectorXF CandG = artic->CandG;
MatrixXF S = MatrixXF::Zero(NJ,NJ+6);
S.block(0,6,NJ,NJ) = MatrixXF::Identity(NJ,NJ);
VectorXF generalizedContactForce = VectorXF::Zero(NJ+6);
for (int i=0; i<NS; i++) {
generalizedContactForce += SupportJacobians[i].transpose()*fs.segment(6*i,6);
}
VectorXF qdd2 = H.inverse()*(S.transpose() * tau + generalizedContactForce- CandG);
//cout << "qdd2 = " << qdd2.transpose() << endl << endl << endl;
//cout << "CandG " << CandG.transpose() << endl;
//cout << "Gen Contact Force " << generalizedContactForce.transpose() << endl;
cout << "hdot " << (CentMomMat*qdd + cmBias).transpose() << endl;
cout << "cmBias " << cmBias.transpose() << endl;
cout << "qd " << qd.transpose() << endl;
//VectorXF qdd3 = H.inverse()*(S.transpose() * tau - CandG);
//FullPivHouseholderQR<MatrixXF> fact(H);
cout <<"fNet \t" << (fs.segment(3,3) + fs.segment(9,3)).transpose() << endl;
Vector3F g;
g << 0,0,-9.81;
cout <<"hdot - mg\t" << (CentMomMat*qdd + cmBias).segment(3,3).transpose() - totalMass * g.transpose()<< endl;
exit(-1);
}
// Old Debug Code
{
VectorXF generalizedContactForce = VectorXF::Zero(NJ+6);
Matrix6F X;
MatrixXF Jac;
X.setIdentity();
for (int i=0; i<NS; i++) {
int linkIndex = SupportIndices[i];
artic->computeJacobian(linkIndex,X,Jac);
dmRigidBody * link = (dmRigidBody *) artic->getLink(linkIndex);
for (int j=0; j< link->getNumForces(); j++) {
dmForce * f = link->getForce(j);
Vector6F fContact;
f->computeForce(artic->m_link_list[linkIndex]->link_val2,fContact.data());
generalizedContactForce += Jac.transpose()*fContact;
}
}
cout << "J' f = " << generalizedContactForce.transpose() << endl;
VectorXF qdd3 = H.inverse()*(S.transpose() * tau - CandG + generalizedContactForce);
cout << "qdd3 = " << qdd3.transpose() << endl;
VectorXF state = VectorXF::Zero(2*(NJ+7));
state.segment(0,NJ+7) = q;
state.segment(NJ+7,NJ+7) = qdDm;
VectorXF stateDot = VectorXF::Zero(2*(NJ+7));
//Process qdds
artic->dynamics(state.data(),stateDot.data());
//
VectorXF qdds = VectorXF::Zero(NJ+6);
qdds.segment(0,6) = stateDot.segment(NJ+7,6);
//cout << "w x v " << cr3(qd.segment(0,3))*qd.segment(3,3) << endl;
qdds.segment(3,3) -= cr3(qdDm.segment(0,3))*qdDm.segment(3,3);
qdds.segment(6,NJ) = stateDot.segment(NJ+7*2,NJ);
Matrix3F ics_R_fb;
copyRtoMat(artic->m_link_list[0]->link_val2.R_ICS,ics_R_fb);
qdds.head(3) = ics_R_fb.transpose() * qdds.head(3);
qdds.segment(3,3) = ics_R_fb.transpose() * qdds.segment(3,3);
cout << "qdds = " << qdds.transpose() << endl;
//cout << "CandG " << endl << CandG << endl;
//cout << setprecision(6);
//cout << "I_0^C = " << endl << artic->H.block(0,0,6,6) << endl;
//exit(-1);
}
}
#endif
/*{
MatrixXF H = artic->H;
VectorXF CandG = artic->CandG;
MatrixXF S = MatrixXF::Zero(NJ,NJ+6);
S.block(0,6,NJ,NJ) = MatrixXF::Identity(NJ,NJ);
VectorXF generalizedContactForce = VectorXF::Zero(NJ+6);
for (int i=0; i<NS; i++) {
generalizedContactForce += SupportJacobians[i].transpose()*fs.segment(6*i,6);
}
qdd = H.inverse()*(S.transpose() * tau + generalizedContactForce- CandG);
cout << setprecision(8);
cout << "fs " << endl << fs << endl;
cout << "qdd " << endl << qdd << endl;
}
exit(-1);*/
static int numTimes = 0;
numTimes++;
//Float dummy;
//cin >> dummy;
//if (numTimes == 2) {
// exit(-1);
//}
//exit(-1);
}
