PhysicsAction

physicsAction.h

//
//  physicsAction.h
//  akivalipshitz
//
//  Created by Akiva Lipshitz on 8/2/14.
//  Copyright (c) 2014 Apportable. All rights reserved.
//

#import "CCActionInterval.h"
#import <objc/message.h>

CGPoint trueVelocityWith(CGPoint velStart, CGPoint velEnd);
CGFloat totalDistanceWith(NSArray *array, BOOL looped);
BOOL ccpIsEqual(CGPoint p1, CGPoint p2);
@interface physicsAction : CCActionInterval <NSCopying> {
    NSArray *trajectory;
    int index;
    CGFloat increment;
@public
    BOOL(^_shouldContinue)();

    NSTimer *timer;
    long double currentIndex;
}
@property BOOL standBy;
+(id)actionWithDuration:(CCTime)d trajectory:(NSArray *)array target:(CCNode *)targ;
+(id)actionWithDuration:(CCTime)d trajectory:(NSArray *)array target:(CCNode *)targ andBlock:(BOOL(^)())block;
+ (NSArray *)drawBezierFrom:(CGPoint)from to:(CGPoint)to controlA:(CGPoint)a controlB:(CGPoint)b;

-(physicsAction *)initWithDuration:(CCTime)d trajectory:(NSArray *)array target:(CCNode *)targ;
-(physicsAction *)initWithDuration:(CCTime)d trajectory:(NSArray *)array target:(CCNode *)targ andBlock:(BOOL(^)())block;
-(void)recalculateWithArray:(NSArray *)newTrajectory;
-(void)stopSequence;
@end

physicsAction.m

//
//  physicsAction.m
//  akivalipshitz
//
//  Created by Akiva Lipshitz on 8/2/14.
//  Copyright (c) 2014 Apportable. All rights reserved.
//
//
//  physicsAction.m
//  akivalipshitz
//
//  Created by Akiva Lipshitz on 8/2/14.
//  Copyright (c) 2014 Apportable. All rights reserved.
//
#import "physicsAction.h"
#import "starConnecter.h"
#define fuzzyEqual(number,extent,comparator) (comparator + extent > number) && (number >  comparator - extent)

CGPoint trueVelocityWith(CGPoint velStart, CGPoint velEnd) {
    CGPoint firstStep = ccpSub(velStart, velEnd);
    firstStep = ccpSub(ccp(0, 0),firstStep);
    return firstStep;
}
CGFloat totalDistanceWith(NSArray *array, BOOL looped) {
    CGFloat total = 0;
    for (int i = 0; i < [array count];i++) {
        int x = i+1;
        int j = i;
        if (x == [array count]) {
            if (looped) {
                x = 0;
            }
            else {
                x = i;
            }
        }
        CGPoint p = [array[j]CGPointValue];
        CGPoint p2 = [array[x] CGPointValue];
        CGFloat dist = ccpDistance(p, p2);
        total = total + dist;
    }
    return total;
}
BOOL ccpIsEqual(CGPoint p1, CGPoint p2) {
    if (p1.x == p2.x && p1.y == p2.y) {
        return TRUE;
    }
    return NO;
}
@implementation physicsAction {
    CCDrawNode *Drawer;
    CGFloat totalLength;
    __weak CCNode *ownTarget;
}
@synthesize standBy;
+(id)actionWithDuration:(CCTime)d trajectory:(NSArray *)array target:(CCNode *)targ{
    return [[self alloc]initWithDuration:d trajectory:array target:targ];
}

+(id)actionWithDuration:(CCTime)d trajectory:(NSArray *)array target:(CCNode *)targ andBlock:(BOOL(^)())block {
    return [[physicsAction alloc] initWithDuration:d trajectory:array target:(CCNode *)targ andBlock:block];
}
-(physicsAction *)initWithDuration:(CCTime)d trajectory:(NSArray *)array target:(CCNode *)targ{
    if (self = [super initWithDuration:d]) {
        trajectory = [[NSArray alloc] init];
        trajectory = array;
        increment =  d / totalDistanceWith(array, NO);
        totalLength = totalDistanceWith(array, NO);
        ownTarget = targ;
        index = 0;

    }
    return self;
}
-(physicsAction *)initWithDuration:(CCTime)d trajectory:(NSArray *)array target:(CCNode *)targ andBlock:(BOOL(^)())block {
    if (self == [self initWithDuration:d trajectory:array target:(CCNode *)targ]) {
        _shouldContinue = block;
    }
    return self;
}
-(void)startWithTarget:(id)target {
    timer = [NSTimer scheduledTimerWithTimeInterval:increment
                                             target:self selector:@selector(stepTimer) userInfo:nil repeats:YES];
    //    physicsAction *newAct = self;
    //    CCActionCallBlock *stop = [CCActionCallBlock actionWithBlock:^{
    //        [newAct->timer invalidate];
    //    }];
    //    CCActionDelay *del = [CCActionDelay actionWithDuration:_duration];
    //    CCActionSequence *seq = [CCActionSequence actionWithArray:@[del,stop]];
    //    [target runAction:seq];
    [super startWithTarget:target];
    CCNode *newtarget = ((CCNode *)target);
    Drawer = [[CCDrawNode alloc] init];
    [newtarget.parent addChild:Drawer];
    pairOfPoints p;
//    [starConnecter speedDrawWith:p interval:increment points:trajectory target:ownTarget andDrawNode:Drawer];
    NSAssert(newtarget.physicsNode, @"Application crashing. Target must be added to a physicsBody");
    if (!ccpFuzzyEqual(newtarget.position, [trajectory[0] CGPointValue], 10)) {
        CGPoint startVel = trueVelocityWith([trajectory[0] CGPointValue], newtarget.position);
        newtarget.physicsBody.velocity = startVel;
        index++;
    }
    CCNode *truetarg = (CCNode *)_target;
    truetarg.physicsBody.affectedByGravity = NO;
}
-(void)stepTimer {
    
    if (!ccpFuzzyEqual(ownTarget.position , [[trajectory lastObject] CGPointValue], 10)) {
        if (index + 4 >= [trajectory count] -1) {
            [self stopSequence];
        } else {
                index += 4;
            }
        CGPoint nextPoint = [trajectory[index] CGPointValue];
        CGPoint nextVel = trueVelocityWith(ownTarget.position, nextPoint) ;
        //    nextVel.x = nextVel.x + (nextVel.x * dt);
        //    nextVel.y = nextVel.y + (nextVel.y * dt);
        BOOL condition;
        if (_shouldContinue != nil) {
            condition = _shouldContinue();
        } else {
            condition = ccpFuzzyEqual(ownTarget.physicsBody.velocity,ccpMult(ccpNormalize(nextVel), ccpLength(ownTarget.physicsBody.velocity)), ccpLength(ownTarget.physicsBody.velocity) * 10);
        }
        
        if (condition) {
            [ownTarget.physicsBody applyForce:ccpMult(nextVel, 50)];
            ownTarget.physicsBody.velocity = ccpMult(nextVel, 10);
        } else {
            [self stopSequence];
        }
    } else {
        [self stopSequence];
    }
}
-(void)stopSequence{
    [Drawer removeFromParent];
    if (ownTarget.physicsBody.type == CCPhysicsBodyTypeDynamic) {
        ownTarget.physicsBody.affectedByGravity = YES;
    }
    [timer invalidate];
    [self stop];
    
}
-(void)recalculateWithArray:(NSArray *)newTrajectory {
    [Drawer clear];
    CGPoint nearest = [roundedGround calculateNearestPoint:ownTarget.position withArray:newTrajectory];
    int newIndx;
    for (NSValue *cgVal in newTrajectory) {
        if ([cgVal CGPointValue].x == nearest.x && [cgVal CGPointValue].y == nearest.y) {
            newIndx = [newTrajectory indexOfObject:cgVal];
        }
    }
    trajectory = newTrajectory;
    index = newIndx;
    CGPoint nextVel = trueVelocityWith(ownTarget.position, nearest);
    pairOfPoints p;
    [starConnecter speedDrawWith:p interval:increment points:newTrajectory target:ownTarget andDrawNode:Drawer];
    [ownTarget.physicsBody applyForce:ccpMult(nextVel, 50)];
}
#pragma mark Point-Array calculation
+(CGPoint)calculateNearestPoint:(CGPoint)searchpoint withArray:(NSArray *)points{
    float closestDist = INFINITY;
    CGPoint closestPt = ccp(INFINITY, INFINITY);
    for (NSValue *point in points) {
        CGPoint cgPoint = [point CGPointValue];
        //        float dist = sqrt(pow( (cgPoint.x - searchpoint.x), 2) + pow( (cgPoint.y - searchpoint.y), 2));
        CGFloat dist = ccpDistance(cgPoint, searchpoint);
        if (dist < closestDist) {
            closestDist = dist;
            closestPt = cgPoint;
        }
    }
    
    return closestPt;
}

+ (NSArray *)drawBezierFrom:(CGPoint)from to:(CGPoint)to controlA:(CGPoint)a controlB:(CGPoint)b
{
    NSMutableArray *points = [[NSMutableArray alloc] init];
    float qx, qy;
    float q1, q2, q3, q4;
    int plotx, ploty;
    float t = 0.0;
    
    while (t <= 1)
    {
        q1 = t*t*t*-1 + t*t*3 + t*-3 + 1;
        q2 = t*t*t*3 + t*t*-6 + t*3;
        q3 = t*t*t*-3 + t*t*3;
        q4 = t*t*t;
        
        qx = q1*from.x + q2*a.x + q3*b.x + q4*to.x;
        qy = q1*from.y + q2*a.y + q3*b.y + q4*to.y;
        
        plotx = round(qx);
        ploty = round(qy);
        t = t + 0.003;
        
        CGPoint point = ccp(plotx, ploty);
        NSValue *pointVal = [NSValue valueWithCGPoint:point];
        [points addObject:pointVal];
        
    }
    return (NSArray *)points;
}



@end