void AiCarStandard::AnalyzeOthers(float dt, const CarDynamics cars[], const int cars_num)
{
const float half_carlength = 1.25;
const btVector3 throttle_axis = Direction::forward;
for (int i = 0; i != cars_num; ++i)
{
const CarDynamics * icar = &cars[i];
if (icar != car)
{
OtherCarInfo & info = othercars[icar];
// find direction of other cars in our frame
btVector3 relative_position = icar->GetCenterOfMass() - car->GetCenterOfMass();
relative_position = quatRotate(-car->GetOrientation(), relative_position);
// only make a move if the other car is within our distance limit
float fore_position = relative_position.dot(throttle_axis);
btVector3 myvel = quatRotate(-car->GetOrientation(), car->GetVelocity());
btVector3 othervel = quatRotate(-icar->GetOrientation(), icar->GetVelocity());
float speed_diff = othervel.dot(throttle_axis) - myvel.dot(throttle_axis);
const float fore_position_offset = -half_carlength;
if (fore_position > fore_position_offset)
{
const Bezier * othercarpatch = GetCurrentPatch(icar);
const Bezier * mycarpatch = GetCurrentPatch(car);
if (othercarpatch && mycarpatch)
{
Vec3 mypos = ToMathVector<float>(car->GetCenterOfMass());
Vec3 otpos = ToMathVector<float>(icar->GetCenterOfMass());
float my_track_placement = GetHorizontalDistanceAlongPatch(*mycarpatch, mypos);
float their_track_placement = GetHorizontalDistanceAlongPatch(*othercarpatch, otpos);
float speed_diff_denom = clamp(speed_diff, -100, -0.01);
float eta = (fore_position - fore_position_offset) / -speed_diff_denom;
if (!info.active)
info.eta = eta;
else
info.eta = RateLimit(info.eta, eta, 10.f*dt, 10000.f*dt);
info.horizontal_distance = their_track_placement - my_track_placement;
info.fore_distance = fore_position;
info.active = true;
}
else
{
info.active = false;
}
}
else
{
info.active = false;
}
}
}
}
void SpaceObject::fire_laser() {
if(weapons[activeWeapon]->ammo<600)
return;
btTransform trans;
body->getMotionState()->getWorldTransform(trans);
btVector3 from = trans.getOrigin() + quatRotate(trans.getRotation() , btVector3(0,0,-10));
std::cout << body->getOrientation().getX() << "," << body->getOrientation().getY() << "," << body->getOrientation().getZ() << "," << body->getOrientation().getW() << "\n";
btVector3 to = from + quatRotate(trans.getRotation() , btVector3(0,0,-500));
//Perform ray test.
btCollisionWorld::ClosestRayResultCallback rayCallback( from, to );
world->dynamicsWorld->rayTest(from,to,rayCallback);
//get results.
if ( rayCallback.hasHit() ) {
to = rayCallback.m_hitPointWorld;
// to = ((SpaceObject*)rayCallback.m_collisionObject->getUserPointer())->getRigidBody()->getCenterOfMassPosition();
ship_hit = (SpaceObject*)rayCallback.m_collisionObject->getUserPointer();
if(ship_hit->getType() != SKYRISE_TALL && ship_hit->getType()!= SKYRISE_FAT && ship_hit->getType()!= ENDPOINT)
wasHit = true;
} //else, do nothing.
weapons[activeWeapon] -> fireProjectile(from,to);
fireFrom = from;
fireTo = to;
fired = true;
}
btVector3 GetWheelPosition(btScalar displacement_fraction)
{
btVector3 up (0, 0, 1);
btVector3 hinge_end = strut.end - strut.hinge;
btVector3 end_top = strut.top - strut.end;
btVector3 hinge_top = strut.top - strut.hinge;
btVector3 rotaxis = up.cross(hinge_end).normalized();
btVector3 localwheelpos = info.position - strut.end;
btScalar hingeradius = hinge_end.length();
btScalar disp_rad = asin(displacement_fraction * info.travel / hingeradius);
btQuaternion hingerotate(rotaxis, -disp_rad);
btScalar e_angle = angle_from_sides(end_top.length(), hinge_end.length(), hinge_top.length());
hinge_end = quatRotate(hingerotate, hinge_end);
btScalar e_angle_disp = angle_from_sides(end_top.length(), hinge_end.length(), hinge_top.length());
rotaxis = up.cross(end_top).normalized();
mountrot.setRotation(rotaxis, e_angle_disp - e_angle);
localwheelpos = quatRotate(mountrot, localwheelpos);
return localwheelpos + strut.hinge + hinge_end;
}
btVector3 GetWheelPosition(btScalar displacement_fraction)
{
btVector3 rel_uc_uh = (hinge[UPPER_HUB] - hinge[UPPER_CHASSIS]),
rel_lc_lh = (hinge[LOWER_HUB] - hinge[LOWER_CHASSIS]),
rel_uc_lh = (hinge[LOWER_HUB] - hinge[UPPER_CHASSIS]),
rel_lc_uh = (hinge[UPPER_HUB] - hinge[LOWER_CHASSIS]),
rel_uc_lc = (hinge[LOWER_CHASSIS] - hinge[UPPER_CHASSIS]),
rel_lh_uh = (hinge[UPPER_HUB] - hinge[LOWER_HUB]),
localwheelpos = (info.position - hinge[LOWER_HUB]);
btScalar radlc = angle_from_sides(rel_lc_lh.length(), rel_uc_lc.length(), rel_uc_lh.length());
btScalar lrotrad = -displacement_fraction * info.travel / rel_lc_lh.length();
btScalar radlcd = radlc + lrotrad;
btScalar d_uc_lh = side_from_angle(radlcd, rel_lc_lh.length(), rel_uc_lc.length());
btScalar radlh = angle_from_sides(rel_lc_lh.length(), rel_lh_uh.length(), rel_lc_uh.length());
btScalar dradlh = (angle_from_sides(rel_lc_lh.length(), d_uc_lh, rel_uc_lc.length()) +
angle_from_sides(d_uc_lh, rel_lh_uh.length(), rel_uc_uh.length()));
btVector3 axiswd = -rel_lc_lh.cross(rel_lh_uh);
btQuaternion hingerot(axis[LOWER_CHASSIS], lrotrad);
mountrot.setRotation(axiswd, radlh - dradlh);
rel_lc_lh = quatRotate(hingerot, rel_lc_lh);
localwheelpos = quatRotate(mountrot, localwheelpos);
return hinge[LOWER_CHASSIS] + rel_lc_lh + localwheelpos;
}
btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
{
if (i == -1) {
return quatRotate(getWorldToBaseRot(), world_dir);
} else {
return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
}
}
void SpaceObject::rotate(double pitch, double yaw) {
btVector3 localXAxis = quatRotate(body->getOrientation(),btVector3(1,0,0));
localXAxis*=mouseScalePitch*pitch;
body->applyTorque(localXAxis);
btVector3 localYAxis = quatRotate(body->getOrientation(),btVector3(0,1,0));
localYAxis*=mouseScaleYaw*yaw;
body->applyTorque(localYAxis);
return;
}
btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
{
if (i == -1) {
// world to base
return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
} else {
// find position in parent frame, then transform to current frame
return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
}
}
btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
{
btVector3 result = local_dir;
while (i != -1) {
result = quatRotate(getParentToLocalRot(i).inverse() , result);
i = getParent(i);
}
result = quatRotate(getWorldToBaseRot().inverse() , result);
return result;
}
btRaycastBar2 (btScalar ray_length, btScalar z,btScalar max_y)
{
frame_counter = 0;
ms = 0;
max_ms = 0;
min_ms = 9999;
sum_ms_samples = 0;
sum_ms = 0;
dx = 10.0;
min_x = 0;
max_x = 0;
this->max_y = max_y;
sign = 1.0;
btScalar dalpha = 2*SIMD_2_PI/NUMRAYS;
for (int i = 0; i < NUMRAYS; i++)
{
btScalar alpha = dalpha * i;
// rotate around by alpha degrees y
btQuaternion q(btVector3(0.0, 1.0, 0.0), alpha);
direction[i] = btVector3(1.0, 0.0, 0.0);
direction[i] = quatRotate(q , direction[i]);
direction[i] = direction[i] * ray_length;
source[i] = btVector3(min_x, max_y, z);
dest[i] = source[i] + direction[i];
dest[i][1]=-1000;
normal[i] = btVector3(1.0, 0.0, 0.0);
}
}
btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
{
btVector3 result = local_pos;
while (i != -1) {
// 'result' is in frame i. transform it to frame parent(i)
result += getRVector(i);
result = quatRotate(getParentToLocalRot(i).inverse(),result);
i = getParent(i);
}
// 'result' is now in the base frame. transform it to world frame
result = quatRotate(getWorldToBaseRot().inverse() ,result);
result += getBasePos();
return result;
}
btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
{
// compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
btScalar xEllipse = btCos(fAngleInRadians);
btScalar yEllipse = btSin(fAngleInRadians);
// Use the slope of the vector (using x/yEllipse) and find the length
// of the line that intersects the ellipse:
// x^2 y^2
// --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
// a^2 b^2
// Do the math and it should be clear.
float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
if (fabs(xEllipse) > SIMD_EPSILON)
{
btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
swingLimit = sqrt(swingLimit2);
}
// convert into point in constraint space:
// note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
btVector3 vSwingAxis(0, xEllipse, -yEllipse);
btQuaternion qSwing(vSwingAxis, swingLimit);
btVector3 vPointInConstraintSpace(fLength,0,0);
return quatRotate(qSwing, vPointInConstraintSpace);
}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
{
// since no frame is given, assume this to be zero angle and just pick rb transform axis
// fixed axis in worldspace
btVector3 rbAxisA1, rbAxisA2;
btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
m_rbAFrame.getOrigin() = pivotInA;
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
//start with free
m_lowerLimit = btScalar(1e30);
m_upperLimit = btScalar(-1e30);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0),m_limit()
{
// since no frame is given, assume this to be zero angle and just pick rb transform axis
// fixed axis in worldspace
btVector3 rbAxisA1, rbAxisA2;
btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
m_rbAFrame.getOrigin() = pivotInA;
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
//-----------------------------------------------------------------------------
void AIBaseController::checkPosition(const Vec3 &point, posData *pos_data,
Vec3 *lc, bool use_front_xyz) const
{
// Convert to local coordinates from the point of view of current kart
btQuaternion q(btVector3(0, 1, 0), -m_kart->getHeading());
Vec3 p = point -
(use_front_xyz ? m_kart->getFrontXYZ() : m_kart->getXYZ());
Vec3 local_coordinates = quatRotate(q, p);
// Save local coordinates for later use if needed
if (lc) *lc = local_coordinates;
if (pos_data == NULL) return;
// lhs: tell whether it's left or right hand side
if (local_coordinates.getX() < 0)
pos_data->lhs = true;
else
pos_data->lhs = false;
// behind: tell whether it's behind or not
if (local_coordinates.getZ() < 0)
pos_data->behind = true;
else
pos_data->behind = false;
pos_data->angle = atan2(fabsf(local_coordinates.getX()),
fabsf(local_coordinates.getZ()));
pos_data->distance = p.length();
} // checkPosition
void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
{
// convert target from body to constraint space
btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
qConstraint.normalize();
// extract "pure" hinge component
btVector3 vNoHinge = quatRotate(qConstraint, vHinge);
vNoHinge.normalize();
btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
qHinge.normalize();
// compute angular target, clamped to limits
btScalar targetAngle = qHinge.getAngle();
if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qHinge = -(qHinge);
targetAngle = qHinge.getAngle();
}
if (qHinge.getZ() < 0)
targetAngle = -targetAngle;
setMotorTarget(targetAngle, dt);
}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
#ifdef _BT_USE_CENTER_LIMIT_
m_limit(),
#endif
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{
m_rbAFrame.getOrigin() = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
btVector3 rbAxisA2;
btScalar projection = axisInA.dot(rbAxisA1);
if (projection >= 1.0f - SIMD_EPSILON) {
rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else if (projection <= -1.0f + SIMD_EPSILON) {
rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else {
rbAxisA2 = axisInA.cross(rbAxisA1);
rbAxisA1 = rbAxisA2.cross(axisInA);
}
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = pivotInB;
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
void SpaceObject::yaw_right() {
if(body->getAngularVelocity().length2()>maxOmega){
return;
}
btVector3 localYAxis = quatRotate(body->getOrientation(),btVector3(0,-1,0));
localYAxis*=scalingOmega;
body->applyTorque(localYAxis);
}
void SpaceObject::roll_right() {
if((body->getAngularVelocity().length2())>(maxOmega*100.0)){
return;
}
btVector3 localZAxis = quatRotate(body->getOrientation(),btVector3(0,0,-1));
localZAxis*=30.0*scalingOmega;
body->applyTorque(localZAxis);
}
void SpaceObject::ascend() {
if(body->getLinearVelocity().length2()>maxVelocity){
return;
}
btVector3 localYAxis = quatRotate(body->getOrientation(),btVector3(0,1,0));
localYAxis*=scalingAcceleration;
body->applyCentralImpulse(localYAxis);
}
void MyCharacterController::updateUpAxis(const glm::quat& rotation) {
btVector3 oldUp = _currentUp;
_currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS);
if (!_isHovering) {
const btScalar MIN_UP_ERROR = 0.01f;
if (oldUp.distance(_currentUp) > MIN_UP_ERROR) {
_rigidBody->setGravity(DEFAULT_GRAVITY * _currentUp);
}
}
}
void LinkState::propagateFK(LinkState *p, JointState *j)
{
if (p == NULL && j == NULL)
{
rel_frame_.setIdentity();
abs_position_.setValue(0, 0, 0);
abs_orientation_.setValue(0, 0, 0, 1);
abs_velocity_.setValue(0, 0, 0);
abs_rot_velocity_.setValue(0, 0, 0);
}
else
{
assert(p);
assert(j);
abs_position_ =
p->abs_position_
+ quatRotate(p->abs_orientation_, link_->origin_xyz_)
+ j->getTranslation();
tf::Quaternion rel_or(link_->origin_rpy_[2], link_->origin_rpy_[1], link_->origin_rpy_[0]);
abs_orientation_ = p->abs_orientation_ * j->getRotation() * rel_or;
abs_orientation_.normalize();
abs_velocity_ =
p->abs_velocity_
+ cross(p->abs_rot_velocity_, link_->origin_xyz_)
+ j->getTransVelocity();
abs_rot_velocity_ = p->abs_rot_velocity_ + quatRotate(abs_orientation_, j->getRotVelocity());
// Computes the relative frame transform
rel_frame_.setIdentity();
rel_frame_ *= tf::Transform(tf::Quaternion(0,0,0), link_->origin_xyz_);
rel_frame_ *= j->getTransform();
rel_frame_ *= tf::Transform(tf::Quaternion(link_->origin_rpy_[2],
link_->origin_rpy_[1],
link_->origin_rpy_[0]));
tf::Vector3 jo = j->getTransform().getOrigin();
}
}
btVector3 btMultiBody::getAngularMomentum() const
{
int num_links = getNumLinks();
// TODO: would be better not to allocate memory here
btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
compTreeLinkVelocities(&omega[0], &vel[0]);
rot_from_world[0] = base_quat;
btVector3 result = quatRotate(rot_from_world[0].inverse() , (base_inertia * omega[0]));
for (int i = 0; i < num_links; ++i) {
rot_from_world[i+1] = links[i].cached_rot_parent_to_this * rot_from_world[links[i].parent+1];
result += (quatRotate(rot_from_world[i+1].inverse() , (links[i].inertia * omega[i+1])));
}
return result;
}
void System::drawAll(){
if(updateDraw){
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_DEPTH_TEST);
glFrontFace(GL_CCW);
glCullFace(GL_BACK);
glEnable(GL_CULL_FACE);
glDepthFunc(GL_LEQUAL);
glClearDepth(1.0);
glPointSize(2);
glLoadIdentity();
float4 pitch_quat=CreateFromAxisAngle(cross(dir,camera_up), camera_pitch);
float4 heading_quat=CreateFromAxisAngle(camera_up, camera_heading);
dir=quatRotate(dir,normalize(mult(pitch_quat,heading_quat)));
camera_pos+=camera_pos_delta;
look_at=camera_pos+dir*1;
camera_heading*=.5;
camera_pitch*=.5;
camera_pos_delta*=.5;
gluLookAt(
camera_pos.x, camera_pos.y, camera_pos.z,
look_at.x, look_at.y, look_at.z,
camera_up.x, camera_up.y, camera_up.z);
for(int i=0; i<mSystem->Get_bodylist()->size(); i++){
ChBody* abody=mSystem->Get_bodylist()->at(i);
if(abody->GetCollisionModel()->GetShapeType()==SPHERE){
drawSphere((abody)->GetPos(),(abody)->GetPos_dt().Length(), mSphereRadius);
}
if(abody->GetCollisionModel()->GetShapeType()==BOX){
drawBox(abody,mSphereRadius,mGPUSys);
}
if(abody->GetCollisionModel()->GetShapeType()==ELLIPSOID){
drawSphere(abody,mGPUSys);
}
if(abody->GetCollisionModel()->GetShapeType()==TRIANGLEMESH){
glColor3f (0,0,0);
drawTriMesh(TriMesh,(abody));
}
}
#if defined( _WINDOWS )
Sleep( 30 );
#else
usleep( 30 * 1000 );
#endif
glutSwapBuffers();
}
}
void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
{
int num_links = getNumLinks();
// Calculates the velocities of each link (and the base) in its local frame
omega[0] = quatRotate(base_quat ,getBaseOmega());
vel[0] = quatRotate(base_quat ,getBaseVel());
for (int i = 0; i < num_links; ++i) {
const int parent = links[i].parent;
// transform parent vel into this frame, store in omega[i+1], vel[i+1]
SpatialTransform(btMatrix3x3(links[i].cached_rot_parent_to_this), links[i].cached_r_vector,
omega[parent+1], vel[parent+1],
omega[i+1], vel[i+1]);
// now add qidot * shat_i
omega[i+1] += getJointVel(i) * links[i].axis_top;
vel[i+1] += getJointVel(i) * links[i].axis_bottom;
}
}
static PyObject *
Quaternion_quatRotate(bulletphysics_QuaternionObject *self, PyObject *args, PyObject *kwds) {
bulletphysics_QuaternionObject *py_quaternion = NULL;
bulletphysics_Vector3Object *py_vector3 = NULL;
if (!PyArg_ParseTuple(args, "OO", &py_quaternion, &py_vector3)) {
return NULL;
}
pybulletphysics_checktype(py_quaternion, Quaternion);
pybulletphysics_checktype(py_vector3, Vector3);
return new_pyvector3_from_vector(quatRotate(*(py_quaternion->quaternion), *(py_vector3->vector)));
}
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const
{
btVector3 idl = getLocalInertia();
btVector3 omega1 = getAngularVelocity();
btQuaternion q = getWorldTransform().getRotation();
// Convert to body coordinates
btVector3 omegab = quatRotate(q.inverse(), omega1);
btMatrix3x3 Ib;
Ib.setValue(idl.x(),0,0,
0,idl.y(),0,
0,0,idl.z());
btVector3 ibo = Ib*omegab;
// Residual vector
btVector3 f = step * omegab.cross(ibo);
btMatrix3x3 skew0;
omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]);
btVector3 om = Ib*omegab;
btMatrix3x3 skew1;
om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]);
// Jacobian
btMatrix3x3 J = Ib + (skew0*Ib - skew1)*step;
// btMatrix3x3 Jinv = J.inverse();
// btVector3 omega_div = Jinv*f;
btVector3 omega_div = J.solve33(f);
// Single Newton-Raphson update
omegab = omegab - omega_div;//Solve33(J, f);
// Back to world coordinates
btVector3 omega2 = quatRotate(q,omegab);
btVector3 gf = omega2-omega1;
return gf;
}
void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
{
m_qTarget = q;
// clamp motor target to within limits
{
btScalar softness = 1.f;//m_limitSoftness;
// split into twist and cone
btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
// clamp cone
if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
{
btScalar swingAngle, swingLimit; btVector3 swingAxis;
computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
if (fabs(swingAngle) > SIMD_EPSILON)
{
if (swingAngle > swingLimit*softness)
swingAngle = swingLimit*softness;
else if (swingAngle < -swingLimit*softness)
swingAngle = -swingLimit*softness;
qTargetCone = btQuaternion(swingAxis, swingAngle);
}
}
// clamp twist
if (m_twistSpan >= btScalar(0.05f))
{
btScalar twistAngle; btVector3 twistAxis;
computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
if (fabs(twistAngle) > SIMD_EPSILON)
{
// eddy todo: limitSoftness used here???
if (twistAngle > m_twistSpan*softness)
twistAngle = m_twistSpan*softness;
else if (twistAngle < -m_twistSpan*softness)
twistAngle = -m_twistSpan*softness;
qTargetTwist = btQuaternion(twistAxis, twistAngle);
}
}
m_qTarget = qTargetCone * qTargetTwist;
}
}
// Movement for free moving cameras
void _Camera::HandleMove(const btVector3 &Direction, float Speed) {
switch(Type) {
case FREEMOVE: {
if(!Direction.isZero()) {
btVector3 Move(Direction);
Move.normalize();
Move *= Speed;
btQuaternion Orientation(Game.Camera->getOrientation().x, Game.Camera->getOrientation().y, Game.Camera->getOrientation().z, Game.Camera->getOrientation().w);
Velocity = quatRotate(Orientation, Move);
}
else
Velocity.setZero();
} break;
}
}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
btVector3& axisInA,btVector3& axisInB)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
m_angularOnly(false),
m_enableAngularMotor(false)
{
m_rbAFrame.getOrigin() = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
btVector3 rbAxisA2;
btScalar projection = axisInA.dot(rbAxisA1);
if (projection >= 1.0f - SIMD_EPSILON) {
rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else if (projection <= -1.0f + SIMD_EPSILON) {
rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else {
rbAxisA2 = axisInA.cross(rbAxisA1);
rbAxisA1 = rbAxisA2.cross(axisInA);
}
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = pivotInB;
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
//start with free
m_lowerLimit = btScalar(1e30);
m_upperLimit = btScalar(-1e30);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
}
void BulletDS::updateCollisionObjects() const
{
for(CollisionObjects::iterator ico = _collisionObjects->begin();
ico != _collisionObjects->end(); ++ ico)
{
SP::btCollisionObject collisionObject = boost::get<0>((*ico).second);
OffSet offset = boost::get<1>((*ico).second);
SiconosVector& q = *_q;
DEBUG_EXPR(q.display());
/* with 32bits input ... 1e-7 */
assert(fabs(sqrt(pow(q(3), 2) + pow(q(4), 2) +
pow(q(5), 2) + pow(q(6), 2)) - 1.) < 1e-7);
btQuaternion rbase = btQuaternion(q(4), q(5), q(6), q(3));
btVector3 boffset = btVector3(offset[0], offset[1], offset[2]);
btVector3 rboffset = quatRotate(rbase, boffset);
collisionObject->getWorldTransform().setOrigin(btVector3(q(0)+rboffset[0],
q(1)+rboffset[1],
q(2)+rboffset[2]));
collisionObject->getWorldTransform().getBasis().
setRotation(rbase * btQuaternion(offset[4], offset[5],
offset[6], offset[3]));
/* is this needed ? */
collisionObject->setActivationState(ACTIVE_TAG);
collisionObject->activate();
//_collisionObject->setContactProcessingThreshold(BT_LARGE_FLOAT);
}
}