Expression<Vector>::Ptr UnitZ_Rotation::derivativeExpression(int i) {
int nr = getDep<Rotation>(i,argument);
if (nr==1) {
return Constant<Vector>(Vector::Zero());
} else {
return argument->derivativeExpression(i) * unit_z( argument );
}
}
TEST(TVector3Test, CrossProductCheck)
{
TVector3<float> unit_x(1,0,0);
TVector3<float> unit_y(0,1,0);
TVector3<float> unit_z(0,0,1);
EXPECT_EQ(unit_z.Cross(unit_x), unit_y);
}
void setupFrameBlacksheep(Params& params)
{
/*
Motor placement:
x
(2) | (0)
|
------------ y
|
(1) | (3)
|
*/
//set up arm lengths
//dimensions are for Team Blacksheep Discovery (http://team-blacksheep.com/products/product:98)
params.rotor_count = 4;
std::vector<real_T> arm_lengths;
Vector3r unit_z(0, 0, -1); //NED frame
// relative to Forward vector in the order (0,3,1,2) required by quad X pattern
// http://ardupilot.org/copter/_images/MOTORS_QuadX_QuadPlus.jpg
arm_lengths.push_back(0.22f);
arm_lengths.push_back(0.255f);
arm_lengths.push_back(0.22f);
arm_lengths.push_back(0.255f);
// note: the Forward vector is actually the "x" axis, and the AngleAxisr rotation is pointing down and is left handed, so this means the rotation
// is counter clockwise, so the vector (arm_lengths[i], 0) is the X-axis, so the CCW rotations to position each arm correctly are listed below:
// See measurements here: http://diydrones.com/profiles/blogs/arducopter-tbs-discovery-style (angles reversed because we are doing CCW rotation)
std::vector<real_T> arm_angles;
arm_angles.push_back(-55.0f);
arm_angles.push_back(125.0f);
arm_angles.push_back(55.0f);
arm_angles.push_back(-125.0f);
// quad X pattern
std::vector<RotorTurningDirection> rotor_directions;
rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW);
rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW);
rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW);
rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW);
// data from
// http://dronesvision.net/team-blacksheep-750kv-motor-esc-set-for-tbs-discovery-fpv-quadcopter/
//set up mass
params.mass = 2.0f; //can be varied from 0.800 to 1.600
real_T motor_assembly_weight = 0.052f; // weight for TBS motors
real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;
// the props we are using a E-Prop, which I didn't find in UIUC database, but this one is close:
// http://m-selig.ae.illinois.edu/props/volume-2/plots/ef_130x70_static_ctcp.png
params.rotor_params.C_T = 0.11f;
params.rotor_params.C_P = 0.047f;
params.rotor_params.max_rpm = 9500;
params.rotor_params.calculateMaxThrust();
//set up dimensions of core body box or abdomen (not including arms).
params.body_box.x() = 0.20f; params.body_box.y() = 0.12f; params.body_box.z() = 0.04f;
real_T rotor_z = 2.5f / 100;
//computer rotor poses
params.rotor_poses.clear();
for (uint i = 0; i < 4; i++)
{
Quaternionr angle(AngleAxisr(arm_angles[i] * M_PIf / 180, unit_z));
params.rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[i], 0, rotor_z), angle, true), unit_z, rotor_directions[i]);
};
//compute inertia matrix
computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);
}
void ProcessInput() {
// Ugly mouse handling
if (captureMouse) {
POINT cs;
GetCursorPos(&cs);
m_Player->phi += -(cs.x - 500)/1000.0f;
m_Player->theta += -(cs.y - 500)/1000.0f;
SetCursorPos(500, 500);
}
if (mouseDown == true) {
//mouseDown = false;
int3 tmpId;
int3 tmpOffset;
int tmpSide;
if (s_Render->FindBlock(m_Player->eyepos, m_Player->dir, 128, tmpId, tmpOffset, tmpSide) == 1) {
if (controlKey == true) {
switch (tmpSide) {
case Render::PX: tmpOffset.x += 1; break;
case Render::NX: tmpOffset.x -= 1; break;
case Render::PY: tmpOffset.y += 1; break;
case Render::NY: tmpOffset.y -= 1; break;
case Render::PZ: tmpOffset.z += 1; break;
case Render::NZ: tmpOffset.z -= 1; break;
}
s_World->AddBlock(tmpId, tmpOffset, Block::LAVA);
}
else {
s_World->RemoveBlock(tmpId, tmpOffset);
}
}
}
if (m_Player->theta < 0.01) m_Player->theta = 0.0001f; // theta=0 leads to null crossproduct with unit_z
if (m_Player->theta > PI) m_Player->theta = PI-0.0001f;
if (m_Player->phi > 2*PI) m_Player->phi -= 2*PI;
if (m_Player->phi < 0) m_Player->phi += 2*PI;
m_Player->dir = spher2car(m_Player->theta, m_Player->phi);
// keyboard
if (keys['W'] == TRUE) {
m_Player->eyepos = m_Player->eyepos + m_Player->dir/0.5;
#ifdef APPLE
keys['W'] = FALSE;
#endif
}
if (keys['S'] == TRUE) {
m_Player->eyepos = m_Player->eyepos - m_Player->dir/0.5;
#ifdef APPLE
keys['S'] = FALSE;
#endif
}
float3 unit_z(0, 0, 1);
float3 crpd = cross_prod(unit_z, m_Player->dir);
normalize(crpd);
// strafe
if (keys['A'] == TRUE) {
m_Player->eyepos = m_Player->eyepos + crpd/2.0;
#ifdef APPLE
keys['A'] = FALSE;
#endif
}
if (keys['D'] == TRUE) {
m_Player->eyepos = m_Player->eyepos - crpd/2.0;
#ifdef APPLE
keys['D'] = FALSE;
#endif
}
// shaders
if (keys['V'] == TRUE) {
keys['V'] = FALSE;
s_Shader->UseProgram(Shader::BLUE);
#ifdef APPLE
keys['V'] = FALSE;
#endif
}
if (keys['F'] == TRUE) {
keys['F'] = FALSE;
s_Shader->UseProgram(Shader::NUL);
#ifdef APPLE
keys['F'] = FALSE;
#endif
}
}
