Calculate the collision penalty for a given velocity vector

@param vcand
sampled velocity
@param dvel
desired velocity
@param minPenalty
threshold penalty for early out
481889
private float processSample(float[] vcand, float cs, float[] pos, float rad, float[] vel, float[] dvel,
            float minPenalty, ObstacleAvoidanceDebugData debug) {
        // penalty for straying away from the desired and current velocities
        float vpen = m_params.weightDesVel * (vDist2D(vcand, dvel) * m_invVmax);
        float vcpen = m_params.weightCurVel * (vDist2D(vcand, vel) * m_invVmax);

        // find the threshold hit time to bail out based on the early out penalty
        // (see how the penalty is calculated below to understnad)
        float minPen = minPenalty - vpen - vcpen;
        float tThresold = (m_params.weightToi / minPen - 0.1f) * m_params.horizTime;
        if (tThresold - m_params.horizTime > -Float.MIN_VALUE)
            return minPenalty; // already too much

        // Find min time of impact and exit amongst all obstacles.
        float tmin = m_params.horizTime;
        float side = 0;
        int nside = 0;

        for (int i = 0; i < m_ncircles; ++i) {
            ObstacleCircle cir = m_circles[i];

            // RVO
            float[] vab = vScale(vcand, 2);
            vab = vSub(vab, vel);
            vab = vSub(vab, cir.vel);

            // Side
            side += clamp(Math.min(vDot2D(cir.dp, vab) * 0.5f + 0.5f, vDot2D(cir.np, vab) * 2), 0.0f, 1.0f);
            nside++;

            SweepCircleCircleResult sres = sweepCircleCircle(pos, rad, vab, cir.p, cir.rad);
            if (!sres.intersection)
                continue;
            float htmin = sres.htmin, htmax = sres.htmax;

            // Handle overlapping obstacles.
            if (htmin < 0.0f && htmax > 0.0f) {
                // Avoid more when overlapped.
                htmin = -htmin * 0.5f;
            }

            if (htmin >= 0.0f) {
                // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
                if (htmin < tmin) {
                    tmin = htmin;
                    if (tmin < tThresold)
                        return minPenalty;
                }
            }
        }

        for (int i = 0; i < m_nsegments; ++i) {
            ObstacleSegment seg = m_segments[i];
            float htmin = 0;

            if (seg.touch) {
                // Special case when the agent is very close to the segment.
                float[] sdir = vSub(seg.q, seg.p);
                float[] snorm = new float[3];
                snorm[0] = -sdir[2];
                snorm[2] = sdir[0];
                // If the velocity is pointing towards the segment, no collision.
                if (vDot2D(snorm, vcand) < 0.0f)
                    continue;
                // Else immediate collision.
                htmin = 0.0f;
            } else {
                Tupple2<Boolean, Float> ires = isectRaySeg(pos, vcand, seg.p, seg.q);
                if (!ires.first)
                    continue;
                htmin = ires.second;
            }

            // Avoid less when facing walls.
            htmin *= 2.0f;

            // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
            if (htmin < tmin) {
                tmin = htmin;
                if (tmin < tThresold)
                    return minPenalty;
            }
        }

        // Normalize side bias, to prevent it dominating too much.
        if (nside != 0)
            side /= nside;

        float spen = m_params.weightSide * side;
        float tpen = m_params.weightToi * (1.0f / (0.1f + tmin * m_invHorizTime));

        float penalty = vpen + vcpen + spen + tpen;
        // Store different penalties for debug viewing
        if (debug != null)
            debug.addSample(vcand, cs, penalty, vpen, vcpen, spen, tpen);

        return penalty;
    }
341201
private void triggerOnReleaseActionsWhileVerticalDrag(float yVel) {
    if (yVel < 0 && yVel <= -Y_MIN_VELOCITY) {
      draggableView.maximize();
    } else if (yVel > 0 && yVel >= Y_MIN_VELOCITY) {
      draggableView.minimize();
    } else {
      if (draggableView.isDragViewAboveTheMiddle()) {
        draggableView.maximize();
      } else {
        draggableView.minimize();
      }
    }
  }
1054237
function(){
        this.constraint.restitution=this.mixRestitution(this.shape1.restitution,this.shape2.restitution);
        this.constraint.friction=this.mixFriction(this.shape1.friction,this.shape2.friction);
        var numBuffers=this.manifold.numPoints;
        var i = numBuffers;
        while(i--){
        //for(var i=0;i<numBuffers;i++){
            var b=this.buffer[i];
            var p=this.points[i];
            b.lp1X=p.localPoint1.x;
            b.lp1Y=p.localPoint1.y;
            b.lp1Z=p.localPoint1.z;
            b.lp2X=p.localPoint2.x;
            b.lp2Y=p.localPoint2.y;
            b.lp2Z=p.localPoint2.z;
            b.impulse=p.normalImpulse;
        }
        this.manifold.numPoints=0;
        this.detector.detectCollision(this.shape1,this.shape2,this.manifold);
        var num=this.manifold.numPoints;
        if(num==0){
            this.touching=false;
            return;
        }
        this.touching=true;
        i = num;
        while(i--){
        //for(i=0; i<num; i++){
            p=this.points[i];
            var lp1x=p.localPoint1.x;
            var lp1y=p.localPoint1.y;
            var lp1z=p.localPoint1.z;
            var lp2x=p.localPoint2.x;
            var lp2y=p.localPoint2.y;
            var lp2z=p.localPoint2.z;
            var index=-1;
            var minDistance=0.0004;
            var j = numBuffers;
            while(j--){
            //for(var j=0;j<numBuffers;j++){
                b=this.buffer[j];
                var dx=b.lp1X-lp1x;
                var dy=b.lp1Y-lp1y;
                var dz=b.lp1Z-lp1z;
                var distance1=dx*dx+dy*dy+dz*dz;
                dx=b.lp2X-lp2x;
                dy=b.lp2Y-lp2y;
                dz=b.lp2Z-lp2z;
                var distance2=dx*dx+dy*dy+dz*dz;
                if(distance1<distance2){
                    if(distance1<minDistance){
                        minDistance=distance1;
                        index=j;
                    }
                }else{
                    if(distance2<minDistance){
                        minDistance=distance2;
                        index=j;
                    }
                }
            }
            if(index!=-1){
                var tmp=this.buffer[index];
                this.buffer[index]=this.buffer[--numBuffers];
                this.buffer[numBuffers]=tmp;
                p.normalImpulse=tmp.impulse;
                p.warmStarted=true;
            }else{
                p.normalImpulse=0;
                p.warmStarted=false;
            }
        }
    }
374664
function(v) {
			var pt = this._firstPT,
				min = 0.000001,
				val;
			while (pt) {
				val = pt.c * v + pt.s;
				if (pt.r) {
					val = Math.round(val);
				} else if (val < min && val > -min) {
					val = 0;
				}
				if (pt.f) {
					pt.t[pt.p](val);
				} else {
					pt.t[pt.p] = val;
				}
				pt = pt._next;
			}
			if (this._target.cacheID) {
				this._target.updateCache();
			}
		}
770619
private CollisionResult computeCollision(CollisionCategory category,
                                             double sh,
                                             double sv,
                                             double sx,
                                             double sy,
                                             int max)
    {
        double x = sh;
        double y = sv;
        double ox = x;
        double oy = y;

        boolean collX = false;
        boolean collY = false;

        CollisionResult last = null;
        for (int cur = 0; cur < max; cur++)
        {
            CollisionResult current = computeCollision(category, ox, oy, x, y);
            if (current != null)
            {
                last = current;
                if (current.getX() != null)
                {
                    x = current.getX().doubleValue();
                    collX = true;
                }
                else
                {
                    collX = false;
                }
                if (current.getY() != null)
                {
                    y = current.getY().doubleValue();
                    oy = y;
                }
            }
            else
            {
                collX = false;
            }

            if (!collX)
            {
                ox = x;
                x += sx;
            }

            current = computeCollision(category, ox, oy, x, y);
            if (current != null)
            {
                last = current;
                if (current.getX() != null)
                {
                    x = current.getX().doubleValue();
                }
                if (current.getY() != null)
                {
                    y = current.getY().doubleValue();
                    collY = true;
                }
                else
                {
                    collY = false;
                }
            }
            else
            {
                collY = false;
            }

            if (!collY)
            {
                oy = y;
                y += sy;
            }
        }
        if (category.isGlue())
        {
            if (last == null)
            {
                last = computeCollision(category, ox, oy, x, y - 1);
            }
            if (last == null)
            {
                last = computeCollision(category, ox, oy, x, y - 2);
            }
        }
        return last;
    }
695171
static boolean pointInPolygon(float[] pt, float[] verts, int nverts) {
        // TODO: Replace pnpoly with triArea2D tests?
        int i, j;
        boolean c = false;
        for (i = 0, j = nverts - 1; i < nverts; j = i++) {
            int vi = i * 3;
            int vj = j * 3;
            if (((verts[vi + 2] > pt[2]) != (verts[vj + 2] > pt[2])) && (pt[0] < (verts[vj + 0] - verts[vi + 0])
                    * (pt[2] - verts[vi + 2]) / (verts[vj + 2] - verts[vi + 2]) + verts[vi + 0])) {
                c = !c;
            }
        }
        return c;
    }
138088
function (p, p1, maxIndex, minIndex) {
		var points = this._originalPoints,
			p2, p3;

		minIndex = minIndex || 0;

		// Check all previous line segments (beside the immediately previous) for intersections
		for (var j = maxIndex; j > minIndex; j--) {
			p2 = points[j - 1];
			p3 = points[j];

			if (L.LineUtil.segmentsIntersect(p, p1, p2, p3)) {
				return true;
			}
		}

		return false;
	}
934030
int filterClosePoints(double tolerance, boolean b_remove_last_vertices, boolean only_polygons) {
		int res = 0;
		for (int geometry = getFirstGeometry(); geometry != -1; geometry = getNextGeometry(geometry)) {
			int gt = getGeometryType(geometry);
			if (!Geometry.isMultiPath(gt))
				continue;
			if (only_polygons && gt != GeometryType.Polygon)
				continue;

			boolean b_polygon = getGeometryType(geometry) == Geometry.GeometryType.Polygon;

			for (int path = getFirstPath(geometry); path != -1;) {
				// We go from the start to the half of the path first, then we
				// go from the end to the half of the path.
				int vertex_counter = 0;
				for (int vertex = getFirstVertex(path); vertex_counter < getPathSize(path) / 2;) {
					int next = getNextVertex(vertex);
					if (next == -1)
						break;
					int vindex = getVertexIndex(vertex);
					Segment seg = getSegmentFromIndex_(vindex);
					double length = 0;
					if (seg != null) {
						length = seg.calculateLength2D();
					} else {
						int vindex_next = getVertexIndex(next);
						length = m_vertices._getShortestDistance(vindex,
								vindex_next);
					}

					if (length <= tolerance) {
						if (length == 0) {
							if (res == 0)
								res = -1;
						} else
							res = 1;

						if (next != getLastVertex(path)) {
							transferAllDataToTheVertex(next, vertex);
							removeVertex(next, true);
						}
					} else {
						vertex = getNextVertex(vertex);
					}
					vertex_counter++;
				}

				int first_vertex = getFirstVertex(path);
				for (int vertex = isClosedPath(path) ? first_vertex
						: getLastVertex(path); getPathSize(path) > 0;) {
					int prev = getPrevVertex(vertex);
					if (prev != -1) {
						int vindex_prev = getVertexIndex(prev);
						Segment seg = getSegmentFromIndex_(vindex_prev);
						double length = 0;
						if (seg != null) {
							length = seg.calculateLength2D();
						} else {
							int vindex = getVertexIndex(vertex);
							length = m_vertices._getShortestDistance(vindex,
									vindex_prev);
						}

						if (length <= tolerance) {
							if (length == 0) {
								if (res == 0)
									res = -1;
							} else
								res = 1;

							transferAllDataToTheVertex(prev, vertex);
							removeVertex(prev, false);
							if (first_vertex == prev)
								first_vertex = getFirstVertex(path);
						} else {
							vertex = getPrevVertex(vertex);
							if (vertex == first_vertex)
								break;
						}
					} else {
						removeVertex(vertex, true);// remove the last vertex in
													// the path
						if (res == 0)
							res = -1;
						break;
					}
				}

				int path_size = getPathSize(path);
				if (b_remove_last_vertices
						&& (b_polygon ? path_size < 3 : path_size < 2)) {
					path = removePath(path);
					res = path_size > 0 ? 1 : (res == 0 ? -1 : res);
				} else
					path = getNextPath(path);
			}
		}

		return res;
	}
376496
private void applyConstraints()
    {
        final Map<Tile, Collection<CollisionFormula>> toRemove = new HashMap<>();
        for (int v = 0; v < map.getInTileHeight(); v++)
        {
            for (int h = 0; h < map.getInTileWidth(); h++)
            {
                final Tile tile = map.getTile(h, v);
                if (tile != null)
                {
                    final TileCollision tileCollision = tile.getFeature(TileCollision.class);
                    toRemove.put(tile, checkConstraints(tileCollision, h, v));
                }
            }
        }
        for (final Entry<Tile, Collection<CollisionFormula>> current : toRemove.entrySet())
        {
            final Tile tile = current.getKey();
            final TileCollision tileCollision = tile.getFeature(TileCollision.class);
            for (final CollisionFormula formula : current.getValue())
            {
                tileCollision.removeCollisionFormula(formula);
            }
        }
    }
1352256
private double preDistance(NumberVector v1, NumberVector v2, final int start, final int end) {
    double agg = 0.;
    for(int d = start; d < end; d++) {
      final double xd = v1.doubleValue(d), yd = v2.doubleValue(d);
      final double delta = xd >= yd ? xd - yd : yd - xd;
      agg += MathUtil.powi(delta, intp);
    }
    return agg;
  }
846823
public static boolean checkCollision(CmsPositionBean posA, CmsPositionBean posB, int margin) {

        // check for non collision is easier
        if ((posA.getLeft() - margin) >= (posB.getLeft() + posB.getWidth())) {
            // posA is right of posB
            return false;
        }
        if ((posA.getLeft() + posA.getWidth()) <= (posB.getLeft() - margin)) {
            // posA is left of posB
            return false;
        }
        if ((posA.getTop() - margin) >= (posB.getTop() + posB.getHeight())) {
            // posA is bellow posB
            return false;
        }
        if ((posA.getTop() + posA.getHeight()) <= (posB.getTop() - margin)) {
            // posA is above posB
            return false;
        }

        // in any other case the position rectangles collide
        return true;
    }
652290
function(){

            var isX
                ,hash
                ,tr1
                ,tr2
                ,bound
                ,list
                ,len
                ,i
                ,j
                ,c
                // determine which axis is the last we need to check
                ,collisionFlag = 1 << (dof.z + 1) << (dof.y + 1) << (dof.x + 1)
                ,encounters = this.encounters
                ,enclen = 0
                ,candidates = this.candidates
                ;

            Physics.util.clearArray( encounters );
            Physics.util.clearArray( candidates );

            for ( var xyz = 0; xyz < maxDof; ++xyz ){

                // is the x coord
                isX = (xyz === 0);
                // get the interval list for this axis
                list = this.intervalLists[ xyz ];

                // for each interval bound
                for ( i = 0, len = list.length; i < len; i++ ){

                    bound = list[ i ];
                    tr1 = bound.tracker;

                    if ( bound.type ){

                        // is a max

                        j = enclen;

                        for ( j = enclen - 1; j >= 0; j-- ){

                            tr2 = encounters[ j ];

                            // if they are the same tracked interval
                            if ( tr2 === tr1 ){

                                // remove the interval from the encounters list
                                // faster than .splice()
                                if ( j < enclen - 1 ) {

                                    encounters[ j ] = encounters.pop();

                                } else {

                                    // encountered a max right after a min... no overlap
                                    encounters.pop();
                                }

                                enclen--;

                            } else {

                                // check if we have flagged this pair before
                                // if it's the x axis, create a pair
                                c = this.getPair( tr1, tr2, isX );

                                if ( c && c.flag < collisionFlag ){

                                    // if it's greater than the axis index, set the flag
                                    // to = 0.
                                    // if not, increment the flag by one.
                                    c.flag = c.flag << (xyz + 1);

                                    // c.flag will equal collisionFlag
                                    // if we've incremented the flag
                                    // enough that all axes are overlapping
                                    if ( c.flag === collisionFlag ){

                                        // overlaps on all axes.
                                        // add it to possible collision
                                        // candidates list for narrow phase

                                        candidates.push( c );
                                    }
                                }
                            }
                        }

                    } else {

                        // is a min
                        // just add this minimum to the encounters list
                        enclen = encounters.push( tr1 );
                    }
                }
            }

            return candidates;
        }
235005
private static void subtractTernaryValues(Vector destination,
                                              TernaryVector source) {
        for (int p : source.positiveDimensions())
            destination.set(p, destination.getValue(p).doubleValue() - 1);
        for (int n : source.negativeDimensions())
            destination.set(n, destination.getValue(n).doubleValue() + 1);
    }
523420
def cmdvel2PosTarget(vel):
    '''
    

    '''
    msg=PositionTarget(
        header=Header(
            stamp=rospy.Time.now(),
            frame_id=''),
    )
    msg.coordinate_frame = 8
    msg.type_mask = 1475
    msg.position.x = vel.px
    msg.position.y = vel.py
    msg.position.z = vel.pz
    msg.velocity.x = vel.vx
    msg.velocity.y = vel.vy
    msg.velocity.z = vel.vz
    msg.acceleration_or_force.x = vel.ax
    msg.acceleration_or_force.y = vel.ay
    msg.acceleration_or_force.z = vel.az
    msg.yaw = vel.yaw
    msg.yaw_rate = vel.yaw_rate
    return msg
852373
def could_collide_hor(self, vpos, adsb_pkt):
        ''''''
        margin = self.asterix_settings.filter_dist_xy
        timeout = self.asterix_settings.filter_time
        alat = adsb_pkt.lat * 1.0e-7
        alon = adsb_pkt.lon * 1.0e-7
        avel = adsb_pkt.hor_velocity * 0.01
        vvel = sqrt(vpos.vx**2 + vpos.vy**2)
        dist = mp_util.gps_distance(vpos.lat, vpos.lon, alat, alon)
        dist -= avel * timeout
        dist -= vvel * timeout
        if dist <= margin:
            return True
        return False
1094667
function checkThatNoObjectsCollide() {
  // Broad-phase collision detection (pairs whose bounding volumes intersect).
  let collisions = collidableStore.getPossibleCollisionsForAllCollidables();

  // Narrow-phase collision detection (pairs that actually intersect).
  collisions = _detectPreciseCollisionsFromCollisions(collisions);

  collisions.forEach(collision => {
    console.warn('Objects still intersect after collision resolution', collision);
  });
}
614828
private void removePoints(double x, double y, Collidable collidable)
    {
        final int minX = (int) Math.floor(x / REDUCE_FACTOR);
        final int minY = (int) Math.floor(y / REDUCE_FACTOR);
        final int maxX = (int) Math.floor((x + collidable.getMaxWidth()) / REDUCE_FACTOR);
        final int maxY = (int) Math.floor((y + collidable.getMaxHeight()) / REDUCE_FACTOR);

        removePoints(minX, minY, maxX, maxY, collidable);
    }