//-----------------------------------------------------------------------------
// Purpose: 
//-----------------------------------------------------------------------------
void CTFWeaponBaseGrenadeProj::Deflected( CBaseEntity *pDeflectedBy, Vector &vecDir )
{
	IPhysicsObject *pPhysicsObject = VPhysicsGetObject();
	if ( pPhysicsObject )
	{
		Vector vecOldVelocity, vecVelocity;

		pPhysicsObject->GetVelocity( &vecOldVelocity, NULL );

		float flVel = vecOldVelocity.Length();

		vecVelocity = vecDir;
		vecVelocity *= flVel;
		AngularImpulse angVelocity( ( 600, random->RandomInt( -1200, 1200 ), 0 ) );

		// Now change grenade's direction.
		pPhysicsObject->SetVelocityInstantaneous( &vecVelocity, &angVelocity );
	}

	CBaseCombatCharacter *pBCC = pDeflectedBy->MyCombatCharacterPointer();

	IncremenentDeflected();
	m_hDeflectOwner = pDeflectedBy;
	SetThrower( pBCC );
	ChangeTeam( pDeflectedBy->GetTeamNumber() );
	if ( !TFGameRules()->IsDeathmatch() )
		m_nSkin = pDeflectedBy->GetTeamNumber() - 2;
	// TODO: Live TF2 adds white trail to reflected pipes and stickies. We need one as well.
}
void SetTwistHandler::handleSimulation(){
    // called when the main script calls: simHandleModule
    if(!_initialized){
        _initialize();
    }

    Eigen::Quaternion < simFloat > orientation; //(x,y,z,w)
    Eigen::Matrix< simFloat, 3, 1> linVelocity((simFloat)_twistCommands.twist.linear.x,
    		(simFloat)_twistCommands.twist.linear.y,
    		(simFloat)_twistCommands.twist.linear.z);
    Eigen::Matrix< simFloat, 3, 1> angVelocity((simFloat)_twistCommands.twist.angular.x,
    		(simFloat)_twistCommands.twist.angular.y,
    		(simFloat)_twistCommands.twist.angular.z);

    // Input velocity is expected to be in body frame but V-Rep expects it to be in world frame.
    if(simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){
        linVelocity = orientation*linVelocity;
	    angVelocity = orientation*angVelocity;
    } else {
        std::stringstream ss;
        ss << "- [" << _associatedObjectName << "] Error getting orientation. " << std::endl;
        ConsoleHandler::printInConsole(ss);
    }

    simResetDynamicObject(_associatedObjectID);

	//Set object velocity
    if (_isStatic){
    	Eigen::Matrix<simFloat, 3, 1> position;
    	simGetObjectPosition(_associatedObjectID, -1, position.data());
    	const simFloat timeStep = simGetSimulationTimeStep();
    	position = position + timeStep*linVelocity;
    	simSetObjectPosition(_associatedObjectID, -1, position.data());
    	const simFloat angle = timeStep*angVelocity.norm();
    	if(angle > 1e-6){
    		orientation = Eigen::Quaternion< simFloat >(Eigen::AngleAxis< simFloat >(timeStep*angVelocity.norm(), angVelocity/angVelocity.norm()))*orientation;
    	}
    	simSetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data());
    } else {
		// Apply the linear velocity to the object
		if(simSetObjectFloatParameter(_associatedObjectID, 3000, linVelocity[0])
			&& simSetObjectFloatParameter(_associatedObjectID, 3001, linVelocity[1])
			&& simSetObjectFloatParameter(_associatedObjectID, 3002, linVelocity[2])==-1) {
			std::stringstream ss;
			ss << "- [" << _associatedObjectName << "] Error setting linear velocity. " << std::endl;;
			ConsoleHandler::printInConsole(ss);
		}
		// Apply the angular velocity to the object
		if(simSetObjectFloatParameter(_associatedObjectID, 3020, angVelocity[0])
			&& simSetObjectFloatParameter(_associatedObjectID, 3021, angVelocity[1])
			&& simSetObjectFloatParameter(_associatedObjectID, 3022, angVelocity[2])==-1) {
			std::stringstream ss;
			ss << "- [" << _associatedObjectName << "] Error setting angular velocity. " << std::endl;;
			ConsoleHandler::printInConsole(ss);
		}
    }

}
void CHL2MP_Player::KickAttack(void)
{
	if (!IsDead())
	{
		CBaseViewModel *vm = GetViewModel(1);

		if (vm)
		{
			int	idealSequence = vm->SelectWeightedSequence(ACT_VM_PRIMARYATTACK);

			if (idealSequence >= 0)
			{
				vm->SendViewModelMatchingSequence(idealSequence);
				m_flNextKickAttack = gpGlobals->curtime + vm->SequenceDuration(idealSequence) - 0.5f;
			}
			QAngle	recoil = QAngle(random->RandomFloat(1.0f, 2.0f), random->RandomFloat(-1.0f, 1.0f), 0);
			this->ViewPunch(recoil);

			// Trace up or down based on where the enemy is...
			// But only if we're basically facing that direction
			Vector vecDirection;
			int kick_maxrange = 120;
			AngleVectors(QAngle(clamp(EyeAngles().x, 20, kick_maxrange), EyeAngles().y, EyeAngles().z), &vecDirection);

			CBaseEntity *pEnemy = MyNPCPointer() ? MyNPCPointer()->GetEnemy() : NULL;
			if (pEnemy)
			{
				Vector vecDelta;
				VectorSubtract(pEnemy->WorldSpaceCenter(), Weapon_ShootPosition(), vecDelta);
				VectorNormalize(vecDelta);

				Vector2D vecDelta2D = vecDelta.AsVector2D();
				Vector2DNormalize(vecDelta2D);
				if (DotProduct2D(vecDelta2D, vecDirection.AsVector2D()) > 0.8f)
				{
					vecDirection = vecDelta;
				}
			}

			Vector vecEnd;
			VectorMA(Weapon_ShootPosition(), 50, vecDirection, vecEnd);
			trace_t tr;
			UTIL_TraceHull(Weapon_ShootPosition(), vecEnd, Vector(-16, -16, -16), Vector(16, 16, 16), MASK_SHOT_HULL, this, COLLISION_GROUP_NONE, &tr);

			// did I hit someone?
			float KickDamageMult = 50 + (1 * ((fabs(GetAbsVelocity().x) + fabs(GetAbsVelocity().y) + fabs(GetAbsVelocity().z)) / 48));
			float KickThrowForceMult = 20 + (1 * ((fabs(GetAbsVelocity().x) + fabs(GetAbsVelocity().y) + fabs(GetAbsVelocity().z)) / 48));

			DevMsg("Kicking at %.2f of damage!\n", KickDamageMult);
			DevMsg("Kicking at %.2f of force!\n", KickThrowForceMult);

			if (tr.m_pEnt)
			{
				if (!(tr.m_pEnt))
				{
					//	return;
				}
				else
				{
					CBasePropDoor *pDoor = dynamic_cast<CBasePropDoor*>((CBaseEntity*)tr.m_pEnt);
					if (pDoor)
					{
						if (pDoor->HasSpawnFlags(SF_BREAKABLE_BY_PLAYER))
						{
							AngularImpulse angVelocity(random->RandomFloat(0, 45), 18, random->RandomFloat(-45, 45));
							pDoor->PlayBreakOpenSound();
							pDoor->BreakDoor(Weapon_ShootPosition(), angVelocity);
							return;
						}
						pDoor->PlayBreakFailSound();
						pDoor->KickFail();
						return;
					}

					CBaseEntity *Victim = this->CheckTraceHullAttack(Weapon_ShootPosition(), vecEnd, Vector(-16, -16, -16), Vector(16, 16, 16), KickDamageMult, DMG_CRUSH, KickThrowForceMult, true);
					if (Victim)
					{
						EmitSound("HL2Player.kick_body");
						return;
					}
				}
			}
			UTIL_TraceLine(Weapon_ShootPosition(), vecEnd, MASK_SHOT_HULL, this, COLLISION_GROUP_NONE, &tr);//IF we hit anything else
			if (tr.DidHit())
			{
				EmitSound("HL2Player.kick_wall");
			}
			else
			{
				EmitSound("HL2Player.kick_fire");
			}

		}
	}
}