vector forceat(vector v)
{
vector vminusp,vminusq;
if (Vequal(v,V(0,0)))
return V(0,0);
else
return Vmult(0.001,Vadd(Vrotate(Vnorm(v),pi/2+pi/32+0*sin(frame*2*pi/10)),
Vadd(inoutattractor(v,p,3,85),inoutattractor(v,q,2,381))));
}
void movepart(part* pa)
{
vector tmp;
tmp=forceat(pa->pos);
brownian(pa);
pa->vel=Vadd(Vmult(dampening,pa->vel),Vmult(strength,tmp));
pa->walkr=pa->walkr*walkdamp+(drand48()*2-1)*walkrrnd;
pa->walkt=pa->walkt+(drand48()*2-1)*walktrnd;
pa->pos=Vadd(pa->pos,Vadd(pa->vel,Cart(pa->walkr,pa->walkt)));
}
void movepart(part* pa)
{
vector tmp;
tmp=forceat(pa->pos);
brownian(pa);
pa->vel=Vadd(Vmult(dampening,pa->vel),Vmult(strength,Vadd(tmp,wibbly(pa))));
pa->pos=Vadd(pa->pos,pa->vel);
if (outofbounds(pa->pos))
randpart(pa);
}
void movepart(part* pa)
{
vector tmp;
tmp=forceat(pa->pos);
brownian(pa);
if (Vequal(pa->pos,q) || Vequal(pa->pos,p))
randpart(pa);
else
pa->vel=Vmult(dampening,Vadd(pa->vel,Vmult(speed,tmp)));
pa->pos=Vadd(pa->pos,pa->vel);
}
void plotarrow(vector u,vector d)
{
vector h; /* Head of arrow */
h=Vadd(u,d);
if (!(Vequal(d,V(0,0))))
BWDrawLine(window2,scrx(u),scry(u),scrx(h),scry(h));
}
void plotarrow(BasicWin window,vector u,vector d)
{
vector h; /* Head of arrow */
if (radius(d)>0.005) {
h=Vadd(u,d);
BWDrawLine(window,scrx(u),scry(u),scrx(h),scry(h));
}
}
void ring_emitter(wavelet *wls, float A, float w, float p, vector loc, vector *units, float radius, unsigned points) {
//currently doesn't do 2d
int i;
vector va, vb;
float sweep = (2*M_PI)/points;
va = Vmk();
vb = Vmk();
for (i=0; i<points; i++) {
Vtimes_scalar(va, units[0], cos(sweep*i));
Vtimes_scalar(vb, units[1], sin(sweep*i));
Vadd(va, va, vb);
Vadd(va, va, loc);
wls[i].A = A;
wls[i].w = w;
wls[i].p = p;
wls[i].loc = Vheap(va);
}
free(va);
free(vb);
}
vector forceat(vector v)
{
int i;
rep r;
vector fa;
fa=holder(v,holderstrength);
for (i=0; i<numrs; i++) {
r=repulsor[i];
fa=Vadd(fa,wibblyrepulsor(v,r.pos,r.str,r.dstr,r.off,r.fre));
}
return fa;
}
ChQuaternion<double> AngleDT_to_QuatDT(AngleSet angset,
const ChVector<double>& mangles,
const ChQuaternion<double>& q) {
ChQuaternion<double> res;
ChQuaternion<double> q2;
ChVector<double> ang1, ang2;
ang1 = Quat_to_Angle(angset, q);
ang2 = Vadd(ang1, Vmul(mangles, CH_LOWTOL));
q2 = Angle_to_Quat(angset, ang2);
res = Qscale(Qsub(q2, q), (1 / CH_LOWTOL));
return res;
}
double ChCollisionUtils::PointTriangleDistance(Vector B,
Vector A1,
Vector A2,
Vector A3,
double& mu,
double& mv,
int& is_into,
Vector& Bprojected) {
// defaults
is_into = 0;
mu = mv = -1;
double mdistance = 10e22;
Vector Dx, Dy, Dz, T1, T1p;
Dx = Vsub(A2, A1);
Dz = Vsub(A3, A1);
Dy = Vcross(Dz, Dx);
double dylen = Vlength(Dy);
if (fabs(dylen) < EPS_TRIDEGEN) // degenerate triangle
return mdistance;
Dy = Vmul(Dy, 1.0 / dylen);
ChMatrix33<> mA;
ChMatrix33<> mAi;
mA.Set_A_axis(Dx, Dy, Dz);
// invert triangle coordinate matrix -if singular matrix, was degenerate triangle-.
if (fabs(mA.FastInvert(mAi)) < 0.000001)
return mdistance;
T1 = mAi.Matr_x_Vect(Vsub(B, A1));
T1p = T1;
T1p.y() = 0;
mu = T1.x();
mv = T1.z();
mdistance = -T1.y();
if (mu >= 0 && mv >= 0 && mv <= 1.0 - mu) {
is_into = 1;
Bprojected = Vadd(A1, mA.Matr_x_Vect(T1p));
}
return mdistance;
}
ChQuaternion<double> AngleDTDT_to_QuatDTDT(AngleSet angset,
const ChVector<double>& mangles,
const ChQuaternion<double>& q) {
ChQuaternion<double> res;
ChQuaternion<double> qa, qb;
ChVector<double> ang0, angA, angB;
double hsquared = CH_LOWTOL;
ang0 = Quat_to_Angle(angset, q);
angA = Vsub(ang0, Vmul(mangles, hsquared));
angB = Vadd(ang0, Vmul(mangles, hsquared));
qa = Angle_to_Quat(angset, angA);
qb = Angle_to_Quat(angset, angB);
res = Qscale(Qadd(Qadd(qa, qb), Qscale(q, -2)), 1 / hsquared);
return res;
}
void ChLinkSpring::UpdateForces(double mytime) {
// Inherit force computation:
// also base class can add its own forces.
ChLinkMarkers::UpdateForces(mytime);
spr_react = 0.0;
Vector m_force;
double deform = Get_SpringDeform();
spr_react = spr_f * mod_f_time->Get_y(ChTime);
spr_react -= (spr_k * mod_k_d->Get_y(deform) * mod_k_speed->Get_y(dist_dt)) * (deform);
spr_react -= (spr_r * mod_r_d->Get_y(deform) * mod_r_speed->Get_y(dist_dt)) * (dist_dt);
m_force = Vmul(Vnorm(relM.pos), spr_react);
C_force = Vadd(C_force, m_force);
}
vector pointonline(vector o,vector d,float s)
{
return Vadd(o,Vmult(s*frame,d));
}
vector Vsub(vector u, vector v)
{
return Vadd(u,Vmult(-1,v));
}
void ChLinkGear::UpdateTime (double mytime)
{
// First, inherit to parent class
ChLinkLock::UpdateTime(mytime);
// Move markers 1 and 2 to align them as gear teeth
ChMatrix33<> ma1;
ChMatrix33<> ma2;
ChMatrix33<> mrotma;
ChMatrix33<> marot_beta;
Vector mx;
Vector my;
Vector mz;
Vector mr;
Vector mmark1;
Vector mmark2;
Vector lastX;
Vector vrota;
Coordsys newmarkpos;
ChFrame<double> abs_shaft1;
ChFrame<double> abs_shaft2;
((ChFrame<double>*)Body1)->TrasformLocalToParent(local_shaft1, abs_shaft1);
((ChFrame<double>*)Body2)->TrasformLocalToParent(local_shaft2, abs_shaft2);
Vector vbdist = Vsub(Get_shaft_pos1(),
Get_shaft_pos2());
Vector Trad1 = Vnorm(Vcross(Get_shaft_dir1(), Vnorm(Vcross(Get_shaft_dir1(),vbdist))));
Vector Trad2 = Vnorm(Vcross(Vnorm(Vcross(Get_shaft_dir2(),vbdist)), Get_shaft_dir2()));
double dist = Vlenght(vbdist);
// compute actual rotation of the two wheels (relative to truss).
Vector md1 = abs_shaft1.GetA()->MatrT_x_Vect(-vbdist);
md1.z = 0; md1 = Vnorm (md1);
Vector md2 = abs_shaft2.GetA()->MatrT_x_Vect(-vbdist);
md2.z = 0; md2 = Vnorm (md2);
double periodic_a1 = ChAtan2(md1.x, md1.y);
double periodic_a2 = ChAtan2(md2.x, md2.y);
double old_a1 = a1;
double old_a2 = a2;
double turns_a1 = floor (old_a1 / CH_C_2PI);
double turns_a2 = floor (old_a2 / CH_C_2PI);
double a1U = turns_a1 * CH_C_2PI + periodic_a1 + CH_C_2PI;
double a1M = turns_a1 * CH_C_2PI + periodic_a1;
double a1L = turns_a1 * CH_C_2PI + periodic_a1 - CH_C_2PI;
a1 = a1M;
if (fabs(a1U - old_a1) < fabs(a1M - old_a1))
a1 = a1U;
if (fabs(a1L - a1) < fabs(a1M - a1))
a1 = a1L;
double a2U = turns_a2 * CH_C_2PI + periodic_a2 + CH_C_2PI;
double a2M = turns_a2 * CH_C_2PI + periodic_a2;
double a2L = turns_a2 * CH_C_2PI + periodic_a2 - CH_C_2PI;
a2 = a2M;
if (fabs(a2U - old_a2) < fabs(a2M - old_a2))
a2 = a2U;
if (fabs(a2L - a2) < fabs(a2M - a2))
a2 = a2L;
// compute new markers coordsystem alignment
my = Vnorm (vbdist);
mz = Get_shaft_dir1();
mx = Vnorm(Vcross (my, mz));
mr = Vnorm(Vcross (mz, mx));
mz = Vnorm(Vcross (mx, my));
ChVector<> mz2, mx2, mr2, my2;
my2 = my;
mz2 = Get_shaft_dir2();
mx2 = Vnorm(Vcross (my2, mz2));
mr2 = Vnorm(Vcross (mz2, mx2));
ma1.Set_A_axis(mx,my,mz);
// rotate csys because of beta
vrota.x = 0.0; vrota.y = this->beta; vrota.z = 0.0;
mrotma.Set_A_Rxyz(vrota);
marot_beta.MatrMultiply(ma1, mrotma);
// rotate csys because of alpha
vrota.x = 0.0; vrota.y = 0.0; vrota.z = this->alpha;
if (react_force.x < 0) vrota.z = this->alpha;
else vrota.z = -this->alpha;
mrotma.Set_A_Rxyz(vrota);
ma1.MatrMultiply(marot_beta, mrotma);
ma2.CopyFromMatrix(ma1);
// is a bevel gear?
double be = acos(Vdot(Get_shaft_dir1(), Get_shaft_dir2()));
bool is_bevel= true;
if (fabs( Vdot(Get_shaft_dir1(), Get_shaft_dir2()) )>0.96)
is_bevel = false;
// compute wheel radii
// so that:
// w2 = - tau * w1
if (!is_bevel)
{
double pardist = Vdot(mr, vbdist);
double inv_tau = 1.0/tau;
if (!this->epicyclic)
{
r2 = pardist - pardist / (inv_tau + 1.0);
}
else
{
r2 = pardist - (tau * pardist)/(tau-1.0);
}
r1 = r2*tau; }
else
{
double gamma2;
if (!this->epicyclic)
{
gamma2 = be/(tau + 1.0);
}
else
{
gamma2 = be/(-tau + 1.0);
}
double al = CH_C_PI - acos (Vdot(Get_shaft_dir2(), my));
double te = CH_C_PI - al - be;
double fd = sin(te) * (dist/sin(be));
r2 = fd * tan(gamma2);
r1 = r2*tau;
}
// compute markers positions, supposing they
// stay on the ideal wheel contact point
mmark1 = Vadd(Get_shaft_pos2(), Vmul(mr2, r2));
mmark2 = mmark1;
contact_pt = mmark1;
// correct marker 1 position if phasing is not correct
if (this->checkphase)
{
double realtau = tau;
if (this->epicyclic)
realtau = -tau;
double m_delta;
m_delta = - (a2/realtau) - a1 - phase;
if (m_delta> CH_C_PI) m_delta -= (CH_C_2PI); // range -180..+180 is better than 0...360
if (m_delta> (CH_C_PI/4.0)) m_delta = (CH_C_PI/4.0); // phase correction only in +/- 45°
if (m_delta<-(CH_C_PI/4.0)) m_delta =-(CH_C_PI/4.0);
vrota.x = vrota.y = 0.0; vrota.z = - m_delta;
mrotma.Set_A_Rxyz(vrota); // rotate about Z of shaft to correct
mmark1 = abs_shaft1.GetA()->MatrT_x_Vect(Vsub(mmark1, Get_shaft_pos1() ));
mmark1 = mrotma.Matr_x_Vect(mmark1);
mmark1 = Vadd (abs_shaft1.GetA()->Matr_x_Vect(mmark1), Get_shaft_pos1() );
}
// Move Shaft 1 along its direction if not aligned to wheel
double offset = Vdot (this->Get_shaft_dir1(), (contact_pt - this->Get_shaft_pos1()) );
ChVector<> moff = this->Get_shaft_dir1() * offset;
if (fabs (offset) > 0.0001)
this->local_shaft1.SetPos( local_shaft1.GetPos() + Body1->Dir_World2Body(&moff) );
// ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
marker2->SetMotionType(ChMarker::M_MOTION_EXTERNAL);
marker1->SetMotionType(ChMarker::M_MOTION_EXTERNAL);
// move marker1 in proper positions
newmarkpos.pos = mmark1;
newmarkpos.rot = ma1.Get_A_quaternion();
marker1->Impose_Abs_Coord(newmarkpos); //move marker1 into teeth position
// move marker2 in proper positions
newmarkpos.pos = mmark2;
newmarkpos.rot = ma2.Get_A_quaternion();
marker2->Impose_Abs_Coord(newmarkpos); //move marker2 into teeth position
// imposed relative positions/speeds
deltaC.pos = VNULL;
deltaC_dt.pos = VNULL;
deltaC_dtdt.pos = VNULL;
deltaC.rot = QUNIT; // no relative rotations imposed!
deltaC_dt.rot = QNULL;
deltaC_dtdt.rot = QNULL;
}
static void
digitize_spline (spline_type s)
{
coordinate_type start = real_cartesian_to_offset_x (START_POINT (s)),
control1 = real_cartesian_to_offset_x (CONTROL1 (s)),
control2 = real_cartesian_to_offset_x (CONTROL2 (s)),
end = real_cartesian_to_offset_x (END_POINT (s));
/* These a_i are the coefficients of the polynomial
p(t) = a_3 t^3 + a_2 t^2 + a_1 t + a_0,
computed by expanding the Bernshte\u in polynomial
z(t) = (1 - t)^3z_1 + 3(1 - t)^2tz_2 + 3(1 - t)t^2z_3 + t^3z_4,
where z_1 is the starting point of the spline, z_2 and z_3 the
control points, and z_4 the ending point. We have two such
polynomials p(t), one for the x-coordinate, one for the
y-coordinate. So everything here is done with points and vectors,
instead of just numbers.
a_0 = x_0
a_1 = 3(x_1 - x_0)
a_2 = 3(x_0 + x_2 - 2x_1)
a_3 = x_3 - x_0 + 3(x_1 - x_2) */
coordinate_type /* a0 = start, */
a1 = IPmult_scalar (IPsubtractP (control1, start), 3),
a2 = IPmult_scalar (IPsubtractP (IPadd (start, control2),
IPmult_scalar (control1, 2)),
3),
a3 = IPadd (IPsubtractP (end, start),
IPmult_scalar (IPsubtractP (control1, control2), 3));
/* The step size. We want to use the length of the bounding rectangle
to compute this, instead of the distance between the starting point
and the ending point, since the latter will be zero if the spline
is cyclic. */
real factor = int_distance (control1, start)
+ int_distance (control2, control1)
+ int_distance (end, control2)
+ int_distance (start, end),
/* Avoid division by zero, in pathological cases. (We will just
produce one point. */
delta = 1.0 / MAX (factor, 1),
delta_squared = delta * delta,
delta_cubed = delta_squared * delta;
/* The current position. */
coordinate_type p = start,
previous_p = { start.x - 1, 0 };
/* The real current position. */
real_coordinate_type real_p = int_to_real_coord (p);
/* The first three forward differences evaluated at t = 0. */
vector_type d = make_vector (Padd (IPmult_real (a3, delta_cubed),
Padd (IPmult_real (a2, delta_squared),
IPmult_real (a1, delta)))),
d2 = make_vector (Padd (IPmult_real (a3, 6 * delta_cubed),
IPmult_real (a2, 2 * delta_squared))),
d3 = make_vector (IPmult_real (a3, 6 * delta_cubed));
/* Where we will collect the points to output. */
unsigned point_count = 0;
XPoint point_list[(unsigned) (1.0 / delta) + 1];
real t;
for (t = 0.0; t <= 1.0; t += delta)
{
if (!IPequal (p, previous_p))
{
point_list[point_count].x = p.x;
point_list[point_count].y = p.y;
point_count++;
previous_p = p;
}
/* We must keep track of the current position in unrounded
coordinates, so the calculations do not lose precision. */
real_p = Padd_vector (real_p, d);
p.x = ROUND (real_p.x);
p.y = ROUND (real_p.y);
d = Vadd (d, d2);
d2 = Vadd (d2, d3);
/* d3 is constant with respect to t. */
}
#if 0
/* GDB under system V doesn't grok XPoint, so here's a way to print
out the points we find. */
{
unsigned i;
for (i = 0; i < point_count; i++)
printf ("(%d,%d)", point_list[i].x, point_list[i].y);
puts ("");
}
#endif
XDrawPoints (display, pixmap, gc, point_list, point_count,
CoordModeOrigin);
}
void CHAABBcollider::CollideRecurse(
CHAABBTree *o1, int b1,
CHAABBTree *o2, int b2,
eCollMode flag)
{
CHAABB* box1 = o1->child(b1);
CHAABB* box2 = o2->child(b2);
this->num_bv_tests++;
// first thing, see if we're overlapping
static Vector Translation;
Translation = Vsub (this->T, box1->To);
Translation = Vadd (Translation, this->R.Matr_x_Vect(box2->To));
if (!CHAABB::AABB_Overlap(this->R, this->Rabs, Translation, box1, box2))
return;
// if we are, see if we test triangles next
int l1 = box1->IsLeaf();
int l2 = box2->IsLeaf();
//
// CASE TWO LEAVES
//
if (l1 && l2)
{
this->num_geo_tests++;
// transform the points in b2 into space of b1, then compare
ChGeometry* mgeo1 = o1->geometries[box1->GetGeometryIndex()];
ChGeometry* mgeo2 = o2->geometries[box2->GetGeometryIndex()];
bool just_intersect = false;
if (flag==ChNarrowPhaseCollider::ChC_FIRST_CONTACT)
just_intersect = true;
ChGeometryCollider::ComputeCollisions(*mgeo1, &this->R1, &this->T1,
*mgeo2, &this->R2, &this->T2,
*this,
&this->R, &this->T,
just_intersect);
return;
}
// we dont, so decide whose children to visit next
double sz1 = box1->GetSize();
double sz2 = box2->GetSize();
if (l2 || (!l1 && (sz1 > sz2)))
{
int c1 = box1->GetFirstChildIndex();
int c2 = box1->GetSecondChildIndex();
CollideRecurse(o1,c1,o2,b2,flag);
if ((flag == ChC_FIRST_CONTACT) && (this->GetNumPairs() > 0)) return;
CollideRecurse(o1,c2,o2,b2,flag);
}
else
{
int c1 = box2->GetFirstChildIndex();
int c2 = box2->GetSecondChildIndex();
CollideRecurse(o1,b1,o2,c1,flag);
if ((flag == ChC_FIRST_CONTACT) && (this->GetNumPairs() > 0)) return;
CollideRecurse(o1,b1,o2,c2,flag);
}
}
void ChLinkBrake::UpdateForces (double mytime)
{
// First, inherit to parent class
ChLinkLock::UpdateForces(mytime);
if (this->IsDisabled()) return;
// then, if not sticking,
if (this->brake_torque)
{
if (brake_mode == BRAKE_ROTATION)
{
if ( ((ChLinkMaskLF*)mask)->Constr_E3().IsActive() == false)
{
int mdir;
Vector mv_torque = Vmul(VECT_Z, this->brake_torque);
mdir = 0; // clockwise torque
if (Vdot(this->relWvel, mv_torque) > 0.0)
{
mv_torque = Vmul (mv_torque, -1.0); // keep torque always opposed to ang speed.
mdir = 1; // counterclockwise torque
}
if (mdir != this->last_dir)
this->must_stick = TRUE;
this->last_dir = mdir;
// +++ADD TO LINK TORQUE VECTOR
C_torque = Vadd(C_torque, mv_torque);
}
}
if (brake_mode == BRAKE_TRANSLATEX)
{
if ( ((ChLinkMaskLF*)mask)->Constr_X().IsActive() == false)
{
int mdir;
Vector mv_force = Vmul(VECT_X, this->brake_torque);
mdir = 0; // F--> rear motion: frontfacing break force
if (this->relM_dt.pos.x > 0.0)
{
mv_force = Vmul (mv_force, -1.0); // break force always opposed to speed
mdir = 1; // F<-- backfacing breakforce for front motion
}
if (mdir != this->last_dir)
this->must_stick = TRUE;
this->last_dir = mdir;
// +++ADD TO LINK TORQUE VECTOR
C_force = Vadd(C_force, mv_force);
}
}
}
// turn off sticking feature if stick ration not > 1.0
if (this->stick_ratio <= 1.0)
must_stick = FALSE;
}
